From 34e553c9c03fe666dd8133eff11b0f1cb608ca1d Mon Sep 17 00:00:00 2001
From: weijian <weijian.wang@elephantrobotics.com>
Date: Wed, 14 Sep 2022 11:09:39 +0800
Subject: [PATCH] update

---
 .../launch/communication_service.launch       |  8 +--
 .../scripts/mycobot_services.py               | 59 ++++++++-----------
 2 files changed, 30 insertions(+), 37 deletions(-)

diff --git a/mycobot_communication/launch/communication_service.launch b/mycobot_communication/launch/communication_service.launch
index 6fb96fc..b2b21b0 100644
--- a/mycobot_communication/launch/communication_service.launch
+++ b/mycobot_communication/launch/communication_service.launch
@@ -1,13 +1,13 @@
 <launch>
 	<!-- Select connecting device and serial port ，选择连接设备及串口-->
 
-	<arg name="port" default="/dev/ttyAMA0" />
-	<arg name="baud" default="1000000" />
+	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="baud" default="115200" />
 
 
 	<!-- Open communication service ，开启通讯服务-->
 	<node name="mycobot_services" pkg="mycobot_communication" type="mycobot_services.py" output="screen">
-		<param name="ip" type="string" value="$(arg ip)" />
-		<param name="port" type="int" value="$(arg port)" />
+		<param name="port" type="string" value="$(arg port)" />
+		<param name="baud" type="int" value="$(arg baud)" />
     </node>
 </launch>
diff --git a/mycobot_communication/scripts/mycobot_services.py b/mycobot_communication/scripts/mycobot_services.py
index 0ad3c91..0f105a8 100755
--- a/mycobot_communication/scripts/mycobot_services.py
+++ b/mycobot_communication/scripts/mycobot_services.py
@@ -5,25 +5,18 @@ import rospy
 from mycobot_communication.srv import *
 
 from pymycobot.mycobot import MyCobot
-from pymycobot.mycobotsocket import MyCobotSocket
 
-# mc = None
-ms = None
+mc = None
+
 
 def create_handle():
-    # global mc
-    global ms
+    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))
-    # ms = MyCobot(port, baud)
-    ip=rospy.get_param("~ip")
-    port=rospy.get_param("~port")
-    rospy.loginfo("%s,%s" % (ip,port))
-    ms=MyCobotSocket(ip,port)
-    ms.connect()
+    port = rospy.get_param("~port")
+    baud = rospy.get_param("~baud")
+    rospy.loginfo("%s,%s" % (port, baud))
+    mc = MyCobot(port, baud)
 
 
 def create_services():
@@ -49,16 +42,16 @@ def set_angles(req):
     ]
     sp = req.speed
     print('angles1:',angles)
-    if ms:
-        ms.send_angles(angles, sp)
+    if mc:
+        mc.send_angles(angles, sp)
 
     return SetAnglesResponse(True)
 
 
 def get_angles(req):
     """get angles,获取角度"""
-    if ms:
-        angles = ms.get_angles()
+    if mc:
+        angles = mc.get_angles()
         print('angles2:',angles)
         return GetAnglesResponse(*angles)
 
@@ -75,15 +68,15 @@ def set_coords(req):
     sp = req.speed
     mod = req.model
 
-    if ms:
-        ms.send_coords(coords, sp, mod)
+    if mc:
+        mc.send_coords(coords, sp, mod)
 
     return SetCoordsResponse(True)
 
 
 def get_coords(req):
-    if ms:
-        coords = ms.get_coords()
+    if mc:
+        coords = mc.get_coords()
         print('coords:',coords)
         return GetCoordsResponse(*coords)
 
@@ -91,23 +84,23 @@ def get_coords(req):
 def switch_status(req):
     """Gripper switch status"""
     """夹爪开关状态"""
-    if ms:
+    if mc:
         if req.Status:
-            ms.set_gripper_state(0, 80)
+            mc.set_gripper_state(0, 80)
         else:
-            ms.set_gripper_state(1, 80)
+            mc.set_gripper_state(1, 80)
 
     return GripperStatusResponse(True)
 
 
 def toggle_pump(req):
-    if ms:
+    if mc:
         if req.Status:
-            ms.set_basic_output(req.Pin1, 0)
-            ms.set_basic_output(req.Pin2, 0)
+            mc.set_basic_output(req.Pin1, 0)
+            mc.set_basic_output(req.Pin2, 0)
         else:
-            ms.set_basic_output(req.Pin1, 1)
-            ms.set_basic_output(req.Pin2, 1)
+            mc.set_basic_output(req.Pin1, 1)
+            mc.set_basic_output(req.Pin2, 1)
 
     return PumpStatusResponse(True)
 
@@ -139,12 +132,12 @@ def output_robot_message():
     servo_temperature = "unknown"
     atom_version = "unknown"
 
-    if ms:
-        cn = ms.is_controller_connected()
+    if mc:
+        cn = mc.is_controller_connected()
         if cn == 1:
             connect_status = True
         time.sleep(0.1)
-        si = ms.is_all_servo_enable()
+        si = mc.is_all_servo_enable()
         if si == 1:
             servo_infomation = "all connected"