def __init__(self): port_str = rospy.get_param("/hardware_interface/mycobot_port", "default") if port_str == "default": port = subprocess.check_output(['echo -n /dev/ttyUSB*'], shell=True) else: port = subprocess.check_output(['echo -n ' + port_str], shell=True) self.mycobot_ = MyCobot(port) self.mycobot_.power_on() rospy.init_node('mycobot_hw_interface', anonymous=True) self.joint_state_msg_pub_ = rospy.Publisher('joint_states_array', Float32MultiArray, queue_size=10) self.joint_cmd_sub = rospy.Subscriber("joint_cmd_array", Float32MultiArray, self.jointCommandCallback) self.rate_ = rospy.Rate(10) # 10hz self.joint_state_array_ = [] JOINT_NUMBER = 6 for tmp in range(JOINT_NUMBER): # initialize array tmp = 0.0 self.joint_state_array_.append(tmp) self.pre_data_list = [] self.first_flag = True
def __init__(self): port_str = rospy.get_param("/hardware_interface/mycobot_port", "default") if port_str == "default": port = subprocess.check_output(['echo -n /dev/ttyUSB*'], shell=True) else: port = subprocess.check_output(['echo -n ' + port_str], shell=True) self.mycobot_ = MyCobot(port) self.mycobot_.power_on() rospy.init_node('mycobot_bridge', anonymous=True) self.joint_res_pub_ = rospy.Publisher('/joint_res_', Float32MultiArray, queue_size=10) self.joint_cmd_sub = rospy.Subscriber("/joint_cmd_", Float32MultiArray, self._jointCommandCallback) self.rate_ = rospy.Rate(10) self.joint_state_array_ = [] self.pre_data_list = [] self.flag_first = True NUM_JOINT = 6 for tmp_zero in range(NUM_JOINT): tmp_zero = 0.0 self.joint_state_array_.append(tmp_zero)
def __init__(self): super(MycobotTopics, self).__init__() rospy.init_node("mycobot_topics") rospy.loginfo("start ...") port = rospy.get_param("~port", "/dev/ttyUSB0") baud = rospy.get_param("~baud", 115200) rospy.loginfo("%s,%s" % (port, baud)) self.mc = MyCobot(port, baud) self.lock = threading.Lock()
def setup(): print("") global port, mc plist = list(serial.tools.list_ports.comports()) idx = 1 for port in plist: print("{} : {}".format(idx, port)) idx += 1 _in = input("\nPlease input 1 - {} to choice:".format(idx - 1)) port = str(plist[int(_in) - 1]).split(" - ")[0].strip() print(port) print("") baud = 115200 _baud = input("Please input baud(default:115200):") try: baud = int(_baud) except Exception: pass print(baud) print("") DEBUG = False f = input("Wether DEBUG mode[Y/n]:") if f in ["y", "Y", "yes", "Yes"]: DEBUG = True # mc = MyCobot(port, debug=True) mc = MyCobot(port, baud, debug=DEBUG)
def create_handle(): global mc rospy.init_node("mycobot_services") rospy.loginfo("start ...") port = rospy.get_param("~port") baud = rospy.get_param("~baud") rospy.loginfo("%s,%s" % (port, baud)) mc = MyCobot(port, baud)
def listener(): global mc 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()
def connect_mycobot(self): self.prot = port = self.port_list.get() if not port: self.write_log_to_Text("请选择串口") return self.baud = baud = self.baud_list.get() if not baud: self.write_log_to_Text("请选择波特率") return baud = int(baud) try: # self.mycobot = MyCobot(PI_PORT, PI_BAUD) self.mycobot = MyCobot(port, baud) # self.mycobot = MyCobot("/dev/cu.usbserial-0213245D", 115200) self.write_log_to_Text("连接成功 !") except Exception as e: err_log = """\ \r连接失败 !!! \r================================================= {} \r================================================= """.format(e) self.write_log_to_Text(err_log)
def setup(): #机械臂检测函数,选择正确的串口 global mc print("") plist = list(serial.tools.list_ports.comports()) idx = 0 for port in plist: print("{} : {}".format(idx, port)) idx += 1 if idx == 0: print("The connected device was not detected. Please try reconnecting.") exit(1) _in = input("\nPlease input 0 - {} to choice, you can choice many like: '2,1,3':".format(idx)) idxes = _in.split(',') try: idxes = [int(i) for i in idxes] except Exception: print('Error: Input format error.') exit(1) ports = [str(plist[i]).split(' - ')[0].strip() for i in idxes] print(ports) print("") baud = 115200 _baud = input("Please input baud(default:115200):") try: baud = int(_baud) except Exception: pass print(baud) print("") for port in ports: try: mycobot = MyCobot(port, baud) except Exception as e: print(e) exit(1) mc.append(mycobot)
from pymycobot.mycobot import MyCobot def callback(data): #rospy.loginfo(rospy.get_caller_id() + "%s", data.position) # print(data.position) data_list = [] print(data.position) for index, value in enumerate(data.position): data_list.append(value) mc.send_radians(data_list, 80) # time.sleep(0.5) def listener(): rospy.init_node('control_slider', anonymous=True) rospy.Subscriber("joint_states", JointState, callback) # spin() simply keeps python from exiting until this node is stopped rospy.spin() if __name__ == '__main__': print('sart') port = subprocess.check_output(['echo -n /dev/ttyUSB*'], shell=True).decode() print(port) mc = MyCobot(port) print(mc) listener()
This file for test the API if right. Just can run in Linux. """ import time, random, subprocess from pymycobot.mycobot import MyCobot from pymycobot.genre import Angle, Coord if __name__ == "__main__": sys_ = subprocess.check_output(["uname"], shell=True).decode() if not sys_.startswith("Linux"): print("This script just can run on Linux.") exit(0) port = subprocess.check_output(["echo -n /dev/ttyUSB*"], shell=True).decode() mycobot = MyCobot(port) print("Start check api\n") print("::get_angles()") print("==> degrees: {}\n".format(mycobot.get_angles())) time.sleep(0.5) print("::get_radians()") print("==> radians: {}\n".format(mycobot.get_radians())) time.sleep(0.5) print("::send_angles()") mycobot.send_angles([0, 0, 0, 0, 0, 0], 80) print("==> set angles [0,0,0,0,0,0], speed 80\n") print("Is moving: {}".format(mycobot.is_moving()))
marker_.action = marker_.ADD marker_.scale.x = 0.04 marker_.scale.y = 0.04 marker_.scale.z = 0.04 #marker position initial # print(coords) if not coords: coords = [0, 0, 0, 0, 0, 0] rospy.loginfo('error [101]: can not get coord values') marker_.pose.position.x = coords[1] / 1000 * -1 marker_.pose.position.y = coords[0] / 1000 marker_.pose.position.z = coords[2] / 1000 marker_.color.a = 1.0 marker_.color.g = 1.0 pub_marker.publish(marker_) rate.sleep() if __name__ == '__main__': port = subprocess.check_output(['echo -n /dev/ttyUSB*'], shell=True).decode() mycobot = MyCobot(port) try: talker() except rospy.ROSInterruptException: pass
#!/usr/bin/env python2 from pymycobot import mycobot import rospy from std_msgs.msg import String from std_msgs.msg import Float64 from std_msgs.msg import Int8 from pymycobot import PI_PORT, PI_BAUD from pymycobot.mycobot import MyCobot from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus mc = MyCobot(PI_PORT, PI_BAUD) pub = rospy.Publisher("gripper_status", Float64, queue_size=10) # Defining the subscriber publisher def talker(): rospy.init_node("gripper_node") rate = rospy.Rate(5) while not rospy.is_shutdown(): gripper_value = mc.get_gripper_value() print(gripper_value) pub.publish(gripper_value) rate.sleep() if __name__ == '__main__': try: talker() except rospy.ROSInterruptException:
class MycobotTopics(object): def __init__(self): super(MycobotTopics, self).__init__() rospy.init_node("mycobot_topics") rospy.loginfo("start ...") port = rospy.get_param("~port", "/dev/ttyUSB0") baud = rospy.get_param("~baud", 115200) rospy.loginfo("%s,%s" % (port, baud)) self.mc = MyCobot(port, baud) self.lock = threading.Lock() def start(self): pa = threading.Thread(target=self.pub_real_angles) pb = threading.Thread(target=self.pub_real_coords) sa = threading.Thread(target=self.sub_set_angles) sb = threading.Thread(target=self.sub_set_coords) sg = threading.Thread(target=self.sub_gripper_status) sp = threading.Thread(target=self.sub_pump_status) pa.setDaemon(True) pa.start() pb.setDaemon(True) pb.start() sa.setDaemon(True) sa.start() sb.setDaemon(True) sb.start() sg.setDaemon(True) sg.start() sp.setDaemon(True) sp.start() pa.join() pb.join() sa.join() sb.join() sg.join() sp.join() def pub_real_angles(self): pub = rospy.Publisher("mycobot/angles_real", MycobotAngles, queue_size=5) ma = MycobotAngles() while not rospy.is_shutdown(): self.lock.acquire() angles = self.mc.get_angles() self.lock.release() if angles: ma.joint_1 = angles[0] ma.joint_2 = angles[1] ma.joint_3 = angles[2] ma.joint_4 = angles[3] ma.joint_5 = angles[4] ma.joint_6 = angles[5] pub.publish(ma) time.sleep(0.25) def pub_real_coords(self): pub = rospy.Publisher("mycobot/coords_real", MycobotCoords, queue_size=5) ma = MycobotCoords() while not rospy.is_shutdown(): self.lock.acquire() coords = self.mc.get_coords() self.lock.release() if coords: ma.x = coords[0] ma.y = coords[1] ma.z = coords[2] ma.rx = coords[3] ma.ry = coords[4] ma.rz = coords[5] pub.publish(ma) time.sleep(0.25) def sub_set_angles(self): def callback(data): angles = [ data.joint_1, data.joint_2, data.joint_3, data.joint_4, data.joint_5, data.joint_6, ] sp = int(data.speed) self.mc.send_angles(angles, sp) sub = rospy.Subscriber( "mycobot/angles_goal", MycobotSetAngles, callback=callback ) rospy.spin() def sub_set_coords(self): def callback(data): angles = [data.x, data.y, data.z, data.rx, data.ry, data.rz] sp = int(data.speed) model = int(data.model) self.mc.send_coords(angles, sp, model) sub = rospy.Subscriber( "mycobot/coords_goal", MycobotSetCoords, callback=callback ) rospy.spin() def sub_gripper_status(self): def callback(data): if data.Status: self.mc.set_gripper_state(0, 80) else: self.mc.set_gripper_state(1, 80) sub = rospy.Subscriber( "mycobot/gripper_status", MycobotGripperStatus, callback=callback ) rospy.spin() def sub_pump_status(self): def callback(data): if data.Status: self.mc.set_basic_output(2, 0) self.mc.set_basic_output(5, 0) else: self.mc.set_basic_output(2, 1) self.mc.set_basic_output(5, 1) sub = rospy.Subscriber( "mycobot/pump_status", MycobotPumpStatus, callback=callback ) rospy.spin()
for angles in special_angles: mc.send_angles(angles, 80) time.sleep(4) coords = mc.get_coords() time.sleep(0.1) print(coords) """ [-278.1, -16.8, 104.2, 79.82, -81.6, -169.53] [-279.2, -15.2, 117.5, 34.72, -85.83, -124.83] [171.6, -113.1, 229.8, 114.15, -84.21, -24.27] [-211.5, -113.8, 188.5, -89.79, 6.14, -177.87] [190.8, -113.1, 210.7, 104.26, -82.19, -14.39] """ if __name__ == "__main__": time.sleep(1) with open(os.path.dirname(__file__) + "/port.txt") as f: port = f.read().strip().replace("\n", "") print(port) mc = MyCobot(port, boudrate="1000000") # test(mc) # coords = [-279.2, -15.2, 117.5, 34.72, -85.83, -124.83] # mc.send_coords(coords, 80, 1) # exit(0) while True: print(mc.get_coords()) time.sleep(1)
import time, random, subprocess from pymycobot.mycobot import MyCobot # from pythonAPI.mycobot3 import MyCobot as MyCobot3 from pymycobot.genre import Angle, Coord if __name__ == '__main__': port = subprocess.check_output(['echo -n /dev/ttyUSB*'], shell=True).decode() mycobot = MyCobot(port) # port = subprocess.run(['echo -n /dev/ttyUSB*'], # stdout=subprocess.PIPE, # shell=True).stdout.decode('utf-8') # mycobot = MyCobot3(port) print('Start check api\n') color_name = ['red', 'green', 'blue'] color_code = ['ff0000', '00ff00', '0000ff'] print('::ser_led_color()') i = random.randint(0, len(color_code) - 1) mycobot.set_led_color(color_code[i]) print('==>set color {}\n'.format(color_name[i])) time.sleep(0.5) print('::get_angles()') print('==> degrees: {}\n'.format(mycobot.get_angles())) time.sleep(0.5) print('::get_radians()') print('==> radians: {}\n'.format(mycobot.get_radians()))
def init(mc: MyCobot): degrees = [0, 0, 0, 0, 0, 0] mc.send_angles(degrees, 20)
from pymycobot.mycobot import MyCobot import time DEVICE_NAME = '/dev/tty.usbserial-0203B064' mycobot = MyCobot(DEVICE_NAME) mycobot.send_radians([0.0, 0.0, 0.0, 1.3, 0.0, 0.0], 30) buffer = input('input any key: ') mycobot.send_radians([0.0, -0.9, 2.18, 0.17, 0.0, 0.0], 30) time.sleep(2) mycobot.send_radians([2.9, -0.9, 2.18, 0.17, 0.0, 0.0], 30) time.sleep(2) mycobot.send_radians([2.9, 1.3, 1.5, -1.4, 0.0, 0.0], 30) time.sleep(3) mycobot.send_radians([2.9, 1.3, 1.5, -1.4, -1.57, 0.0], 90)
def mov1(mc : MyCobot): init(mc) time.sleep(10) mc.send_angle(Angle.J1.value, 90, 30) time.sleep(5) mc.send_angle(Angle.J1.value, -90, 30) time.sleep(5) mc.send_angle(Angle.J1.value, 0, 30) time.sleep(5) mc.send_angle(Angle.J2.value, 90, 30) time.sleep(5) mc.send_angle(Angle.J2.value, -90, 30) time.sleep(5) mc.send_angle(Angle.J2.value, 0, 30) time.sleep(5) mc.send_angle(Angle.J3.value, 90, 30) time.sleep(5) mc.send_angle(Angle.J3.value, -90, 30) time.sleep(5) mc.send_angle(Angle.J3.value, 0, 30) time.sleep(5) mc.send_angle(Angle.J4.value, 90, 30) time.sleep(5) mc.send_angle(Angle.J4.value, -90, 30) time.sleep(5) mc.send_angle(Angle.J4.value, 0, 30) time.sleep(5) mc.send_angle(Angle.J5.value, 90, 30) time.sleep(5) mc.send_angle(Angle.J5.value, -90, 30) time.sleep(5) mc.send_angle(Angle.J5.value, 0, 30) time.sleep(5) mc.send_angle(Angle.J6.value, 90, 30) time.sleep(5) mc.send_angle(Angle.J6.value, -90, 30) time.sleep(5) mc.send_angle(Angle.J6.value, 0, 30) time.sleep(5)
class MycobotHwInterface: def __init__(self): port_str = rospy.get_param("/hardware_interface/mycobot_port", "default") if port_str == "default": port = subprocess.check_output(['echo -n /dev/ttyUSB*'], shell=True) else: port = subprocess.check_output(['echo -n ' + port_str], shell=True) self.mycobot_ = MyCobot(port) self.mycobot_.power_on() rospy.init_node('mycobot_hw_interface', anonymous=True) self.joint_state_msg_pub_ = rospy.Publisher('joint_states_array', Float32MultiArray, queue_size=10) self.joint_cmd_sub = rospy.Subscriber("joint_cmd_array", Float32MultiArray, self.jointCommandCallback) self.rate_ = rospy.Rate(10) # 10hz self.joint_state_array_ = [] JOINT_NUMBER = 6 for tmp in range(JOINT_NUMBER): # initialize array tmp = 0.0 self.joint_state_array_.append(tmp) self.pre_data_list = [] self.first_flag = True def main_loop(self): while not rospy.is_shutdown(): self.joint_state_msg_sender() self.rate_.sleep() def joint_state_msg_sender(self): angles = self.mycobot_.get_radians() for index, value in enumerate(angles): if index != 2: value *= -1 self.joint_state_array_[index] = value # str = "angles: %s" % angles # rospy.loginfo(str) joint_state_msg = Float32MultiArray(data=self.joint_state_array_) self.joint_state_msg_pub_.publish(joint_state_msg) def jointCommandCallback(self, msg): data_list = [] for index, value in enumerate(msg.data): if index != 2: value *= -1 data_list.append(value) if self.first_flag: for value in data_list: self.pre_data_list.append(value) self.first_flag = False if self.pre_data_list != data_list: rospy.loginfo(rospy.get_caller_id() + "%s", msg.data) self.mycobot_.send_radians(data_list, 80) self.pre_data_list = [] for value in data_list: self.pre_data_list.append(value)
from pymycobot.mycobot import MyCobot import port import move import threading mc = MyCobot(port.port) threadMov = threading.Thread(target=move.mov1, args=(mc, )) threadMov.start() while True: print(mc.get_coords())
class MycobotTest(object): def __init__(self): self.mycobot = None self.win = tkinter.Tk() self.win.title("树莓派版 Mycobot 测试工具") self.win.geometry( "918x511+10+10") # 290 160为窗口大小,+10 +10 定义窗口弹出时的默认展示位置 self.port_label = tkinter.Label(self.win, text="选择串口:") self.port_label.grid(row=0) self.port_list = ttk.Combobox( self.win, width=15, postcommand=self.get_serial_port_list) # #创建下拉菜单 self.get_serial_port_list() # #给下拉菜单设定值 self.port_list.current(0) self.port_list.grid(row=0, column=1) self.baud_label = tkinter.Label(self.win, text="选择波特率:") self.baud_label.grid(row=1) self.baud_list = ttk.Combobox(self.win, width=15) self.baud_list["value"] = ("115200", "1000000") self.baud_list.current(0) self.baud_list.grid(row=1, column=1) # Connect self.connect_label = tkinter.Label(self.win, text="连接mycobot:") self.connect_label.grid(row=2) self.connect = tkinter.Button(self.win, text="连接", command=self.connect_mycobot) self.disconnect = tkinter.Button(self.win, text="断开", command=self.disconnect_mycobot) self.connect.grid(row=3) self.disconnect.grid(row=3, column=1) # Check servo. self.check_label = tkinter.Label(self.win, text="检测连接:") self.check_label.grid(row=4) self.check_btn = tkinter.Button(self.win, text="开始检测", command=self.check_mycobot_servos) self.check_btn.grid(row=4, column=1) # Calibration. self.calibration_num = None self.calibration_label = tkinter.Label(self.win, text="校准舵机:") self.calibration_label.grid(row=5) self.calibration_btn = tkinter.Button(self.win, text="开始校准", command=self.calibration_mycobot) self.calibration_btn.grid(row=5, column=1) # LED. self.set_color_label = tkinter.Label(self.win, text="测试Atom灯板:") self.set_color_label.grid(row=6, columnspan=2) self.color_red = tkinter.Button(self.win, text="设置红色", command=lambda: self.send_color("red")) self.color_green = tkinter.Button( self.win, text="设置绿色", command=lambda: self.send_color("green")) self.color_red.grid(row=7) self.color_green.grid(row=7, column=1) # Aging test. self.aging_stop = False self.movement_label = tkinter.Label(self.win, text="老化循环动作:") self.movement_label.grid(row=8) self.start_btn = tkinter.Button(self.win, text="开始", command=self.start_aging_test) self.start_btn.grid(row=9) self.stop_btn = tkinter.Button(self.win, text="停止", command=self.stop_aging_test) self.stop_btn.grid(row=9, column=1) # Release self.release_btn = tkinter.Button(self.win, text="放松所有电机", command=self.release_mycobot) self.release_btn.grid(row=10) # rectify self.rectify_btn = tkinter.Button(self.win, text="校准pid", command=self.rectify_mycobot) self.rectify_btn.grid(row=10, column=1) # Log output. self.log_label = tkinter.Label(self.win, text="日志:") self.log_label.grid(row=0, column=12) _f = tkinter.Frame(self.win) _bar = tkinter.Scrollbar(_f, orient=tkinter.VERTICAL) self.log_data_Text = tkinter.Text(_f, width=100, height=35, yscrollcommand=_bar.set) _bar.pack(side=tkinter.RIGHT, fill=tkinter.Y) _bar.config(command=self.log_data_Text.yview) self.log_data_Text.pack() # self.log_data_Text.grid(row=1, column=12, rowspan=15, columnspan=10) _f.grid(row=1, column=12, rowspan=15, columnspan=10) def run(self): self.win.mainloop() # run # ============================================================ # Connect method # ============================================================ def connect_mycobot(self): self.prot = port = self.port_list.get() if not port: self.write_log_to_Text("请选择串口") return self.baud = baud = self.baud_list.get() if not baud: self.write_log_to_Text("请选择波特率") return baud = int(baud) try: # self.mycobot = MyCobot(PI_PORT, PI_BAUD) self.mycobot = MyCobot(port, baud) # self.mycobot = MyCobot("/dev/cu.usbserial-0213245D", 115200) self.write_log_to_Text("连接成功 !") except Exception as e: err_log = """\ \r连接失败 !!! \r================================================= {} \r================================================= """.format(e) self.write_log_to_Text(err_log) def disconnect_mycobot(self): if not self.has_mycobot(): return try: del self.mycobot self.mycobot = None self.write_log_to_Text("断开连接成功 !") except AttributeError: self.write_log_to_Text("还没有连接mycobot!!!") # ============================================================ # Function method # ============================================================ def release_mycobot(self): if not self.has_mycobot(): return self.mycobot.release_all_servos() self.write_log_to_Text("Release over.") def check_mycobot_servos(self): if not self.has_mycobot(): return ping_commands = [ [255, 255, 1, 2, 1, 251], [255, 255, 2, 2, 1, 250], [255, 255, 3, 2, 1, 249], [255, 255, 4, 2, 1, 248], [255, 255, 5, 2, 1, 247], [255, 255, 6, 2, 1, 246], [255, 255, 7, 2, 1, 246], ] res = [] for idx, command in enumerate(ping_commands, start=1): self.mycobot._write(command) time.sleep(0.1) if not self.mycobot._read(): res.append(idx) time.sleep(0.1) if res: self.write_log_to_Text("关节 {} 无法通信!!!".format(res)) else: self.write_log_to_Text("所有关节连接正常。") def calibration_mycobot(self): """Calibration button click event. Click to calibrate one motor at a time and calibrate in turn. After all calibration, resume initialization. """ if not self.has_mycobot(): return if not self.calibration_num: self.calibration_num = 0 self.calibration_num += 1 self.mycobot.set_servo_calibration(self.calibration_num) time.sleep(0.1) self.mycobot.send_angle(self.calibration_num, 0, 0) time.sleep(0.1) self.write_log_to_Text("校准电机 %s 结束." % self.calibration_num) if self.calibration_num == 6: self.write_log_to_Text("全部校准完成.") self.calibration_num = None self.rectify_mycobot() self._calibration_test() def send_color(self, color: str): if not self.has_mycobot(): return color_dict = { "red": [255, 0, 0], "green": [0, 255, 0], "blue": [0, 0, 255], } self.mycobot.set_color(*color_dict[color]) self.write_log_to_Text("发送颜色: {}.".format(color)) def start_aging_test(self): if not self.has_mycobot(): return self.aging_stop = False self.aging = threading.Thread(target=self._aging_test, daemon=True) self.aging.start() # self._aging_test() self.write_log_to_Text("开始循环老化测试 ...") def stop_aging_test(self): try: os.system("sudo systemctl stop aging_test.service") os.system("sudo rm /home/pi/aging_test.sh") os.system("sudo rm /home/pi/Desktop/aging_test.py") os.system("sudo rm /etc/systemd/system/aging_test.service") os.system("sudo systemctl daemon-reload") self.write_log_to_Text("结束循环老化测试.") except: self.write_log_to_Text("结束老化测试失败 !!!") def rectify_mycobot(self): if not self.has_mycobot(): return for i in range(1, 7): self.mycobot.set_servo_data(i, 24, 0) time.sleep(0.1) self.mycobot.set_servo_data(i, 26, 3) time.sleep(0.1) self.mycobot.set_servo_data(i, 27, 3) time.sleep(0.1) time.sleep(0.1) for i in range(1, 7): self.write_log_to_Text( "Read servo {} pid data, 24:{}, 26:{}, 27:{}.".format( i, self.mycobot.get_servo_data(i, 24), self.mycobot.get_servo_data(i, 26), self.mycobot.get_servo_data(i, 27), )) time.sleep(0.1) # ============================================================ # Utils method # ============================================================ def has_mycobot(self): """Check whether it is connected on mycobot""" if not self.mycobot: self.write_log_to_Text("还没有连接mycobot!!!") return False return True def _aging_test(self): """ Aging test thread target. By using in `start_aging_test()` and `stop_aging_test()`. """ # if socket.gethostname() != "pi": # self.write_log_to_Text("老化测试支持 Raspberry OS.") # return aging_test_content_py = textwrap.dedent("""\ #!/usr/bin/python3 from pymycobot.mycobot import MyCobot from pymycobot import PI_PORT, PI_BAUD import time mycobot = MyCobot('%s', %s) def aging_test(): # fast mycobot.set_color(255, 0, 0) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([170, 0, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([-170, 0, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 90, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, -90, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 140, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, -140, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 130, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, -110, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, 165, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, -165, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 180], 95) mycobot.wait(3).send_angles([0, 0, 0, 0, 0, -180], 95) # middle mycobot.set_color(0, 255, 0) mycobot.wait(3).send_angles([0, 0, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([170, 0, 0, 0, 0, 0], 55) mycobot.wait(6.5).send_angles([-170, 0, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 90, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, -90, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 140, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, -140, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 130, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, -110, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, 165, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, -165, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 180], 55) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, -180], 55) # slow mycobot.set_color(0, 0, 255) mycobot.wait(5).send_angles([0, 0, 0, 0, 0, 0], 15) mycobot.wait(7).send_angles([170, 0, 0, 0, 0, 0], 15) mycobot.wait(7).send_angles([-170, 0, 0, 0, 0, 0], 15) mycobot.wait(11).send_angles([0, 0, 0, 0, 0, 0], 15) mycobot.wait(7).send_angles([0, 90, 0, 0, 0, 0], 15) mycobot.wait(7).send_angles([0, -90, 0, 0, 0, 0], 15) mycobot.wait(0).send_angles([0, 0, 0, 0, 0, 0], 15) mycobot.wait(7).send_angles([0, 0, 140, 0, 0, 0], 15) mycobot.wait(7).send_angles([0, 0, -140, 0, 0, 0], 15) mycobot.wait(11).send_angles([0, 0, 0, 0, 0, 0], 15) mycobot.wait(7).send_angles([0, 0, 0, 130, 0, 0], 15) mycobot.wait(7).send_angles([0, 0, 0, -110, 0, 0], 15) mycobot.wait(11).send_angles([0, 0, 0, 0, 0, 0], 15) mycobot.wait(7).send_angles([0, 0, 0, 0, 165, 0], 15) mycobot.wait(7).send_angles([0, 0, 0, 0, -165, 0], 15) mycobot.wait(11).send_angles([0, 0, 0, 0, 0, 0], 15) mycobot.wait(7).send_angles([0, 0, 0, 0, 0, 180], 15) mycobot.wait(7).send_angles([0, 0, 0, 0, 0, -180], 15) if __name__ == '__main__': while True: aging_test() """ % (self.prot, self.baud)) aging_test_content_sh = textwrap.dedent("""\ #!/bin/bash /usr/bin/python3 /home/pi/Desktop/aging_test.py """) aging_test_content_service = textwrap.dedent("""\ [Unit] Description=aging-test [Service] Type=forking User=pi Restart=on-failure RestartSec=2 ExecStart=/home/pi/aging_test.sh [Install] WantedBy=multi-user.target """) os.system('echo "' + aging_test_content_py + '" >> /home/pi/Desktop/aging_test.py') os.system('echo "' + aging_test_content_sh + '" >> /home/pi/aging_test.sh') os.system("sudo chmod +x /home/pi/aging_test.sh") os.system('echo "' + aging_test_content_service + '" >> /home/pi/Desktop/aging_test.service') os.system( "sudo mv /home/pi/Desktop/aging_test.service /etc/systemd/system/aging_test.service" ) os.system("sudo systemctl enable aging_test.service") os.system("sudo systemctl start aging_test.service") def _calibration_test(self): self.write_log_to_Text("开始测试校准.") angles = [0, 0, 0, 0, 0, 0] test_angle = [-20, 20, 0] for i in range(6): for j in range(3): angles[i] = test_angle[j] self.mycobot.send_angles(angles, 0) time.sleep(0.7) self.write_log_to_Text("测试校准结束.") def get_serial_port_list(self): plist = [ str(x).split(" - ")[0].strip() for x in serial.tools.list_ports.comports() ] print(plist) self.port_list["value"] = plist return plist def get_current_time(self): """Get current time with format.""" current_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time())) return current_time def write_log_to_Text(self, logmsg: str): global LOG_NUM current_time = self.get_current_time() logmsg_in = str(current_time) + " " + str(logmsg) + "\n" # 换行 if LOG_NUM <= 18: self.log_data_Text.insert(tkinter.END, logmsg_in) LOG_NUM += len(logmsg_in.split("\n")) # print(LOG_NUM) else: self.log_data_Text.insert(tkinter.END, logmsg_in) self.log_data_Text.yview("end")
from pymycobot.mycobot import MyCobot from port import port mc = MyCobot(port) mc.release_all_servos()
def talker(): rospy.init_node("display", anonymous=True) print("Try connect real mycobot...") port = rospy.get_param("~port", "/dev/ttyAMA0") baud = rospy.get_param("~baud", 115200) print("port: {}, baud: {}\n".format(port, baud)) try: mycobot = MyCobot(port, baud) except Exception as e: print(e) print( """\ \rTry connect mycobot failed! \rPlease check wether connected with mycobot. \rPlease chckt wether the port or baud is right. """ ) exit(1) mycobot.release_all_servos() time.sleep(0.1) print("Rlease all servos over.\n") pub = rospy.Publisher("joint_states", JointState, queue_size=10) pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10) rate = rospy.Rate(30) # 30hz # pub joint state joint_state_send = JointState() joint_state_send.header = Header() joint_state_send.name = [ "joint2_to_joint1", "joint3_to_joint2", "joint4_to_joint3", "joint5_to_joint4", "joint6_to_joint5", "joint6output_to_joint6", ] joint_state_send.velocity = [0] joint_state_send.effort = [] marker_ = Marker() marker_.header.frame_id = "/joint1" marker_.ns = "my_namespace" print("publishing ...") while not rospy.is_shutdown(): joint_state_send.header.stamp = rospy.Time.now() angles = mycobot.get_radians() data_list = [] for index, value in enumerate(angles): data_list.append(value) # rospy.loginfo('{}'.format(data_list)) joint_state_send.position = data_list pub.publish(joint_state_send) coords = mycobot.get_coords() # marker marker_.header.stamp = rospy.Time.now() marker_.type = marker_.SPHERE marker_.action = marker_.ADD marker_.scale.x = 0.04 marker_.scale.y = 0.04 marker_.scale.z = 0.04 # marker position initial # print(coords) if not coords: coords = [0, 0, 0, 0, 0, 0] rospy.loginfo("error [101]: can not get coord values") marker_.pose.position.x = coords[1] / 1000 * -1 marker_.pose.position.y = coords[0] / 1000 marker_.pose.position.z = coords[2] / 1000 marker_.color.a = 1.0 marker_.color.g = 1.0 pub_marker.publish(marker_) rate.sleep()
from pymycobot.mycobot import MyCobot from port import port from pymycobot.genre import Angle mc = MyCobot(port) mc.send_angle(Angle.J1.value, 90, 10)
from pymycobot.mycobot import MyCobot # name of device port = "/dev/ttyUSB0" mc = MyCobot(port) # release mycobot # mc.release_all_servos() # calibrate the sixth servo mc.set_servo_calibration(6)
class MycobotBridge: def __init__(self): port_str = rospy.get_param("/hardware_interface/mycobot_port", "default") if port_str == "default": port = subprocess.check_output(['echo -n /dev/ttyUSB*'], shell=True) else: port = subprocess.check_output(['echo -n ' + port_str], shell=True) self.mycobot_ = MyCobot(port) self.mycobot_.power_on() rospy.init_node('mycobot_bridge', anonymous=True) self.joint_res_pub_ = rospy.Publisher('/joint_res_', Float32MultiArray, queue_size=10) self.joint_cmd_sub = rospy.Subscriber("/joint_cmd_", Float32MultiArray, self._jointCommandCallback) self.rate_ = rospy.Rate(10) self.joint_state_array_ = [] self.pre_data_list = [] self.flag_first = True NUM_JOINT = 6 for tmp_zero in range(NUM_JOINT): tmp_zero = 0.0 self.joint_state_array_.append(tmp_zero) def _joint_state_msg_sender(self): angles = self.mycobot_.get_radians() for index, value in enumerate(angles): if index != 2: value *= -1 self.joint_state_array_[index] = value joint_state_msg = Float32MultiArray(data=self.joint_state_array_) self.joint_res_pub_.publish(joint_state_msg) def _jointCommandCallback(self, msg): data_list = [] for index, value in enumerate(msg.data): if index != 2: value *= -1 data_list.append(value) if self.flag_first: for value in data_list: self.pre_data_list.append(value) self.flag_first = False if self.pre_data_list != data_list: rospy.loginfo(rospy.get_caller_id() + "%s", msg.data) self.mycobot_.send_radians(data_list, 90) self.pre_data_list = [] for value in data_list: self.pre_data_list.append(value) def run(self): while not rospy.is_shutdown(): self._joint_state_msg_sender() self.rate_.sleep()