release v3.1.4

anla-xu · anla-xu · commit c82f67de4fcf · 2023-07-05T16:07:41.000+08:00
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,5 +1,11 @@
 # ChangeLog for pymycobot
 
+# v3.1.4 (2023-7-5)
+
+- release v3.1.4
+- Update set_gservo_round() function.
+- Add pro 600 interface
+
 ## v3.1.3 (2023-06-29)
 
 - release v3.1.3
diff --git a/docs/README.md b/docs/README.md
@@ -1104,10 +1104,17 @@ Set the terminal atom io status
 
 ### set_gservo_round
 
-- **Prototype**: `set_gservo_round()`
+- **Prototype**: `set_gservo_round(angle)`
 
 - **Description**: Drive the 9g steering gear clockwise for one revolution.
 
+- **Parameters**
+
+  - `angle` (`int`) 0 - 255.
+      0 : stop
+      255 : Keep turning
+      1 ~ 254: Based on 30° (1->30°, 2->60°)
+
 ### get_servo_speeds
 
 - **Prototype**: `get_servo_speeds()`
diff --git a/pymycobot/__init__.py b/pymycobot/__init__.py
@@ -18,6 +18,7 @@
 from pymycobot.mybuddybluetooth import MyBuddyBlueTooth
 from pymycobot.mypalletizersocket import MyPalletizerSocket
 from pymycobot.myarm import MyArm
+from pymycobot.elephantrobot import ElephantRobot
 
 __all__ = [
     "MyCobot",
@@ -34,15 +35,16 @@
     "ultraArm",
     "MyPalletizerSocket",
     "MechArm",
-    "MyArm"
+    "MyArm",
+    "ElephantRobot"
 ]
 
 
 if sys.platform == "linux":
     from pymycobot.mybuddyemoticon import MyBuddyEmoticon
     __all__.append("MyBuddyEmoticon")
 
-__version__ = "3.1.3"
+__version__ = "3.1.4"
 __author__ = "Elephantrobotics"
 __email__ = "weiquan.xu@elephantrobotics.com"
 __git_url__ = "https://github.com/elephantrobotics/pymycobot"
diff --git a/pymycobot/elephantrobot.py b/pymycobot/elephantrobot.py
@@ -0,0 +1,303 @@
+#!/usr/bin/python
+
+from socket import *
+import sys
+from multiprocessing import Lock
+import logging
+from pymycobot.log import setup_logging
+
+
+mutex = Lock()
+
+
+class ElephantRobot(object):
+    def __init__(self, host, port, debug=False):
+        # setup connection
+        self.debug = debug
+        setup_logging(self.debug)
+        self.log = logging.getLogger(__name__)
+        self.BUFFSIZE = 2048
+        self.ADDR = (host, port)
+        self.tcp_client = socket(AF_INET, SOCK_STREAM)
+
+    def start_client(self):
+        try:
+            self.tcp_client.connect(self.ADDR)
+            return ""
+        except Exception as e:
+            return e
+
+    def stop_client(self):
+        self.tcp_client.close()
+
+    def send_command(self, command):
+        with mutex:
+            self.tcp_client.send(command.encode())
+            recv_data = self.tcp_client.recv(self.BUFFSIZE).decode()
+            res_str = str(recv_data)
+            print("recv = " + res_str)
+            res_arr = res_str.split(":")
+            if len(res_arr) == 2:
+                return res_arr[1]
+            else:
+                return ""
+
+    def string_to_coords(self, data):
+        data = data.replace("[", "")
+        data = data.replace("]", "")
+        data_arr = data.split(",")
+        if len(data_arr) == 6:
+            try:
+                coords_1 = float(data_arr[0])
+                coords_2 = float(data_arr[1])
+                coords_3 = float(data_arr[2])
+                coords_4 = float(data_arr[3])
+                coords_5 = float(data_arr[4])
+                coords_6 = float(data_arr[5])
+                coords = [coords_1, coords_2, coords_3, coords_4, coords_5, coords_6]
+                return coords
+            except:
+                return self.invalid_coords()
+        return self.invalid_coords()
+
+    def string_to_double(self, data):
+        try:
+            val = float(data)
+            return val
+        except:
+            return -9999.99
+
+    def string_to_int(self, data):
+        try:
+            val = int(data)
+            return val
+        except:
+            return -9999
+
+    def invalid_coords(self):
+        coords = [-1, -2, -3, -4, -1, -1]
+        return coords
+
+    def get_angles(self):
+        command = "get_angles()\n"
+        res = self.send_command(command)
+        return self.string_to_coords(res)
+
+    def get_coords(self):
+        command = "get_coords()\n"
+        res = self.send_command(command)
+        return self.string_to_coords(res)
+
+    def get_speed(self):
+        command = "get_speed()\n"
+        res = self.send_command(command)
+        return self.string_to_double(res)
+
+    def power_on(self):
+        command = "power_on()\n"
+        res = self.send_command(command)
+        return True
+
+    def power_off(self):
+        command = "power_off()\n"
+        res = self.send_command(command)
+        return True
+
+    def start_robot(self):
+        command = "start_robot()\n"
+        res = self.send_command(command)
+        return True
+
+    def check_running(self):
+        command = "check_running()\n"
+        res = self.send_command(command)
+        return res == "1"
+
+    def state_check(self):
+        command = "state_check()\n"
+        res = self.send_command(command)
+        return res == "1"
+
+    def program_open(self, file_path):
+        command = "program_open(" + file_path + ")\n"
+        res = self.send_command(command)
+        return self.string_to_int(res)
+
+    def program_run(self, start_line):
+        command = "program_run(" + str(start_line) + ")\n"
+        res = self.send_command(command)
+        return self.string_to_int(res)
+
+    def read_next_error(self):
+        command = "read_next_error()\n"
+        res = self.send_command(command)
+        return res
+
+    def write_coords(self, coords, speed):
+        command = "set_coords("
+        for item in coords:
+            command += str(item) + ","
+        command += str(speed) + ")\n"
+        self.send_command(command)
+
+    def write_coord(self, axis, value, speed):
+        coords = self.get_coords()
+        if coords != self.invalid_coords():
+            coords[axis] = value
+            self.write_coords(coords, speed)
+
+    def write_angles(self, angles, speed):
+        command = "set_angles("
+        for item in angles:
+            command += str(item) + ","
+        command += str(speed) + ")\n"
+        self.send_command(command)
+
+    def write_angle(self, joint, value, speed):
+        angles = self.get_angles()
+        if angles != self.invalid_coords():
+            angles[joint] = value
+            self.write_angles(angles, speed)
+
+    def set_speed(self, percentage):
+        command = "set_speed(" + str(percentage) + ")\n"
+        self.send_command(command)
+
+    def set_carte_torque_limit(self, axis_str, value):
+        command = "set_torque_limit(" + axis_str + "," + str(value) + ")\n"
+        self.send_command(command)
+
+    def set_upside_down(self, up_down):
+        up = "1"
+        if up_down:
+            up = "0"
+        command = "set_upside_down(" + up + ")\n"
+        self.send_command(command)
+
+    def set_payload(self, payload):
+        command = "set_speed(" + str(payload) + ")\n"
+        self.send_command(command)
+
+    def state_on(self):
+        command = "state_on()\n"
+        self.send_command(command)
+
+    def state_off(self):
+        command = "state_off()\n"
+        self.send_command(command)
+
+    def task_stop(self):
+        command = "task_stop()\n"
+        self.send_command(command)
+
+    def jog_angle(self, joint_str, direction, speed):
+        command = (
+            "jog_angle(" + joint_str + "," + str(direction) + "," + str(speed) + ")\n"
+        )
+        self.send_command(command)
+
+    def jog_coord(self, axis_str, direction, speed):
+        command = (
+            "jog_coord(" + axis_str + "," + str(direction) + "," + str(speed) + ")\n"
+        )
+        self.send_command(command)
+
+    def get_digital_in(self, pin_number):
+        command = "get_digital_in(" + str(pin_number) + ")\n"
+        self.send_command(command)
+
+    def get_digital_out(self, pin_number):
+        command = "get_digital_out(" + str(pin_number) + ")\n"
+        print(command)
+        self.send_command(command)
+
+    def get_joint_current(self, joint_number: int):
+        command = "get_joint_current(" + str(joint_number) + ")\n"
+        print(command)
+        self.send_command(command)
+
+    def set_digital_out(self, pin_number, pin_signal):
+        command = "set_digital_out(" + str(pin_number) + "," + str(pin_signal) + ")\n"
+        self.send_command(command)
+
+    def set_analog_out(self, pin_number, pin_signal):
+        command = "set_analog_out(" + str(pin_number) + "," + str(pin_signal) + ")\n"
+        self.send_command(command)
+
+    def send_feed_override(self, override):
+        command = "set_feed_rate(" + str(override) + ")\n"
+        res = self.send_command(command)
+        return self.string_to_int(res)
+
+    def get_acceleration(self):
+        command = "get_acceleration()\n"
+        res = self.send_command(command)
+        return self.string_to_int(res)
+
+    def set_acceleration(self, acceleration):
+        command = "set_acceleration(" + str(acceleration) + ")\n"
+        self.send_command(command)
+
+    def command_wait_done(self):
+        command = "wait_command_done()\n"
+        self.send_command(command)
+
+    def wait(self, seconds):
+        command = "wait(" + str(seconds) + ")\n"
+        self.send_command(command)
+
+    def assign_variable(self, var_name, var_value):
+        command = 'assign_variable("' + str(var_name) + '",' + str(var_value) + ")\n"
+        self.send_command(command)
+
+    def get_variable(self, var_name):
+        command = 'get_variable("' + str(var_name) + '")\n'
+        return self.send_command(command)
+
+
+if __name__ == "__main__":
+    ep = ElephantRobot("192.168.124.28", 5001)
+    res = ep.start_client()
+    if res != "":
+        print(res)
+        sys.exit(1)
+    print(ep.wait(5))
+    print(ep.get_angles())
+    print(ep.get_coords())
+    print(ep.get_speed())
+    # print(ep.power_on())
+    # print(ep.power_off())
+    print(ep.check_running())
+    print(ep.state_check())
+    print(ep.program_open("a.tax"))
+    print(ep.program_run(0))
+    print(ep.read_next_error())
+    print(ep.write_coords([1, 2, 3, 4, 5, 6], 110))
+    print(ep.write_coord(1, 100, 200))
+    print(ep.write_angles([10, 20, 30, 40, 50, 60], 110))
+    print(ep.write_angle(3, 180, 200))
+    print(ep.set_speed(377))
+    print(ep.set_carte_torque_limit("x", 55))
+    print(ep.set_upside_down(False))
+    print(ep.set_payload(100))
+    print(ep.state_on())
+    print(ep.state_off())
+    print(ep.task_stop())
+    print(ep.jog_angle("j2", 1, 300))
+    print(ep.jog_coord("rY", 0, 200))
+    print(ep.get_digital_in(3))
+    print(ep.get_digital_out(3))
+    print(ep.set_digital_out(3, 1))
+    print(ep.get_digital_out(3))
+    print(ep.set_analog_out(1, 3.5))
+    print(ep.get_acceleration())
+    print(ep.set_acceleration(55))
+    print(ep.command_wait_done())
+    print(ep.get_variable("f"))
+    print(ep.assign_variable("ss", '"eee"'))
+    print(ep.get_joint_current(1))
+
+    ep.stop_client()
+
+    name = input("input:")
+    print(name)
diff --git a/pymycobot/generate.py b/pymycobot/generate.py
@@ -1031,6 +1031,13 @@ def clear_error_information(self):
         """Clear robot error message"""
         return self._mesg(ProtocolCode.CLEAR_ERROR_INFO, has_reply = True)
     
-    def set_gservo_round(self):
-        """Drive the 9g steering gear clockwise for one revolution"""
-        return self._mesg(ProtocolCode.SET_GSERVO_ROUND)
+    def set_gservo_round(self, angle):
+        """Drive the 9g steering gear clockwise for one revolution
+        
+        Args:
+            angle (int): 0 ~ 255
+                0 : stop
+                255 : Keep turning
+                1 ~ 254: Based on 30° (1->30°, 2->60°)
+        """
+        return self._mesg(ProtocolCode.SET_GSERVO_ROUND, angle)
