diff --git a/CHANGELOG.md b/CHANGELOG.md
index c90cb3d..e0ef0ec 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,5 +1,41 @@
# ChangeLog for pymycobot
+## v3.9.1 (2025-3-4)
+
+- release v3.9.1
+- Fix bugs
+
+## v3.9.0 (2025-2-27)
+
+- release v3.9.0
+- Fix bugs
+
+## v3.6.9 (2024-12-26)
+
+- release v3.6.9
+- The set_basic_output interface adds IO 5 to open the suction pump solenoid valve and automatically close it after 0.5S
+- Fix bugs
+
+## v3.6.7 (2024-11-15)
+
+- release v3.6.7
+- Fixed the issue of incorrect naming of 320 electric gripper
+
+## v3.6.5 (2024-10-25)
+
+- release v3.6.5
+- Added 280 set_encoders_drag interface
+
+## v3.6.4 (2024-10-22)
+
+- release v3.6.4
+- Fix set_encoder function BUG
+
+## v3.6.3 (2024-10-17)
+
+- release v3.6.3
+- Fix bug and add new function
+
## v3.6.2 (2024-10-11)
- release v3.6.2
diff --git a/README.md b/README.md
index 124b877..17d2f04 100644
--- a/README.md
+++ b/README.md
@@ -67,6 +67,8 @@ from pymycobot import MechArmSocket
# for mypalletizer 260 machine
from pymycobot import MyPalletizer260
from pymycobot import MyPalletizerSocket
+# for ultraArm P340 machine
+from pymycobot import ultraArmP340
```
The [`demo`](./demo) directory stores some test case files.
@@ -90,4 +92,6 @@ Please go to [here](./docs/README.md).
[myAGV API说明](./docs/myAGV_zh.md) | [myAGV API Description](./docs/myAGV_en.md)
-[myArm_M&C API说明](./docs/myArm_M&C_zh.md) | [myArm_M&C API Description](./docs/myArm_M&C_en.md)
\ No newline at end of file
+[myArm_M&C API说明](./docs/myArm_M&C_zh.md) | [myArm_M&C API Description](./docs/myArm_M&C_en.md)
+
+[ultraArm P340 API说明](./docs/ultraArm_P340_zh.md) | [ultraArm P340 API Description](./docs/ultraArm_P340_en.md)
\ No newline at end of file
diff --git a/demo/Client.py b/demo/Client.py
index 708bdf4..aee9469 100644
--- a/demo/Client.py
+++ b/demo/Client.py
@@ -1,6 +1,6 @@
-from pymycobot import MyCobotSocket
+from pymycobot import MyCobot280Socket
-m = MyCobotSocket("192.168.10.10", "9000")
+m = MyCobot280Socket("192.168.10.10", "9000")
# connect pi
# m.connect()
print(m.get_coords())
diff --git a/demo/Mercury_A1_demo/6D_mouse_serial_port_control.py b/demo/Mercury_A1_demo/6D_mouse_serial_port_control.py
new file mode 100644
index 0000000..fc015be
--- /dev/null
+++ b/demo/Mercury_A1_demo/6D_mouse_serial_port_control.py
@@ -0,0 +1,151 @@
+import pygame
+import time
+from pymycobot.mercury import Mercury
+
+# 初始化机械臂
+mc = Mercury('/dev/ttyAMA1', 115200)
+
+# 检测机械臂是否上电
+if mc.is_power_on() != 1:
+ mc.power_on()
+
+# 设置夹爪透传模式
+mc.set_gripper_mode(0)
+
+THRESHOLD = 0.7 # 鼠标轴触发运动的阈值
+DEAD_ZONE = 0.2 # 归零判断阈值
+
+# jog 坐标运动速度
+jog_speed = 20
+
+# 夹爪速度
+gripper_speed = 70
+
+# 初始点运动速度
+home_speed = 10
+
+# 夹爪状态（默认张开）
+gripper_state = True
+
+# 6D 轴映射关系，鼠标轴 -> 机械臂坐标ID映射
+AXIS_MAPPING = {
+ 0: 2,
+ 1: 1,
+ 2: 3,
+ 3: 4,
+ 4: 5,
+ 5: 6
+}
+
+# 轴运动方向映射
+DIRECTION_MAPPING = {
+ 0: (-1, 1), # 轴0 (Y) -> 负向 -1，正向 1
+ 1: (-1, 1), # 轴1 (X) -> 负向 -1，正向 1
+ 2: (-1, 1), # 轴2 (Z) -> 负向 -1，正向 1
+ 3: (-1, 1), # 轴3 (RX) -> 负向 -1，正向 1
+ 4: (1, -1), # 轴4 (RY) -> 负向 1，正向 -1
+ 5: (1, -1) # 轴5 (RZ) -> 负向 1，正向 -1
+}
+
+
+def handle_mouse_event(jog_callback, stop_callback, gripper_callback):
+ """
+ 监听 6D 鼠标事件，并调用传入的回调函数控制机械臂
+ :param jog_callback: 运动回调函数 (coord_id, direction)
+ :param stop_callback: 停止运动回调函数 (coord_id)
+ :param gripper_callback: 夹爪开合回调函数 ()
+ """
+ pygame.init()
+ pygame.joystick.init()
+
+ if pygame.joystick.get_count() == 0:
+ print("未检测到 6D 鼠标设备！")
+ return
+
+ joystick = pygame.joystick.Joystick(0)
+ joystick.init()
+ print(f"已连接 6D 鼠标: {joystick.get_name()}")
+
+ active_axes = {} # 记录当前运动状态
+ home_active = False # 记录初始点运动状态
+
+ try:
+ while True:
+ for event in pygame.event.get():
+ # 处理轴运动
+ if event.type == pygame.JOYAXISMOTION:
+ axis_id = event.axis
+ value = event.value
+
+ if axis_id in AXIS_MAPPING:
+ coord_id = AXIS_MAPPING[axis_id]
+ pos_dir, neg_dir = DIRECTION_MAPPING[axis_id]
+
+ if value > THRESHOLD and active_axes.get(axis_id) != 1:
+ jog_callback(coord_id, pos_dir)
+ active_axes[axis_id] = 1
+
+ elif value < -THRESHOLD and active_axes.get(axis_id) != -1:
+ jog_callback(coord_id, neg_dir)
+ active_axes[axis_id] = -1
+
+ elif -DEAD_ZONE < value < DEAD_ZONE and axis_id in active_axes:
+ stop_callback(coord_id)
+ del active_axes[axis_id]
+
+ # 处理按键（按钮 0 回到初始点，按钮 1 控制夹爪）
+ elif event.type == pygame.JOYBUTTONDOWN:
+ if event.button == 0: # 按下按钮 0，回到初始点
+ home_callback()
+ home_active = True
+ elif event.button == 1: # 按下按钮 1，切换夹爪状态
+ gripper_callback()
+
+ elif event.type == pygame.JOYBUTTONUP:
+ if event.button == 0 and home_active: # 松开按钮 0，停止初始点运动
+ home_stop_callback()
+ home_active = False
+
+ time.sleep(0.01)
+ except KeyboardInterrupt:
+ print("监听结束")
+ pygame.quit()
+
+
+# 定义回调函数
+def jog_callback(coord_id, direction):
+ """触发机械臂JOG坐标运动"""
+ if direction != 1:
+ direction = 0
+ if coord_id in [1,2,3]:
+ mc.jog_coord(coord_id, direction, jog_speed, _async=True)
+ else:
+ model = [2, 1, 3]
+ model_dir = [0, 1, 1]
+ model_id = model[coord_id-4]
+ model_dire = direction ^ model_dir[coord_id-4]
+ mc.jog_rpy(model_id, model_dire, jog_speed, _async=True)
+
+def stop_callback(coord_id):
+ """停止机械臂运动"""
+ mc.stop()
+
+def gripper_callback():
+ """控制夹爪开合"""
+ global gripper_state
+ gripper_state = not gripper_state
+ flag = 1 if gripper_state else 0
+ mc.set_gripper_state(flag, gripper_speed)
+
+def home_callback():
+ """回到初始点"""
+ mc.send_angles([0, 0, 0, -90, 0, 90, 0], home_speed, _async=True)
+
+def home_stop_callback():
+ """停止回到初始点"""
+ mc.stop()
+
+
+if __name__ == '__main__':
+ # 启动监听
+ handle_mouse_event(jog_callback, stop_callback, gripper_callback)
diff --git a/demo/Mercury_A1_demo/6D_mouse_socket_control.py b/demo/Mercury_A1_demo/6D_mouse_socket_control.py
new file mode 100644
index 0000000..bef5acf
--- /dev/null
+++ b/demo/Mercury_A1_demo/6D_mouse_socket_control.py
@@ -0,0 +1,147 @@
+import pygame
+import time
+from pymycobot import MercurySocket
+
+# 初始化机械臂,IP和端口号需根据实际进行修改
+mc = MercurySocket('192.168.1.4', 9001)
+
+# 设置夹爪透传模式
+mc.set_gripper_mode(0)
+
+THRESHOLD = 0.7 # 鼠标轴触发运动的阈值
+DEAD_ZONE = 0.2 # 归零判断阈值
+
+# jog 坐标运动速度
+jog_speed = 20
+
+# 夹爪速度
+gripper_speed = 70
+
+# 初始点运动速度
+home_speed = 10
+
+# 夹爪状态（默认张开）
+gripper_state = True
+
+# 6D 轴映射关系，鼠标轴 -> 机械臂坐标ID映射
+AXIS_MAPPING = {
+ 0: 2,
+ 1: 1,
+ 2: 3,
+ 3: 4,
+ 4: 5,
+ 5: 6
+}
+
+# 轴运动方向映射
+DIRECTION_MAPPING = {
+ 0: (-1, 1), # 轴0 (Y) -> 负向 -1，正向 1
+ 1: (-1, 1), # 轴1 (X) -> 负向 -1，正向 1
+ 2: (-1, 1), # 轴2 (Z) -> 负向 -1，正向 1
+ 3: (-1, 1), # 轴3 (RX) -> 负向 -1，正向 1
+ 4: (1, -1), # 轴4 (RY) -> 负向 1，正向 -1
+ 5: (1, -1) # 轴5 (RZ) -> 负向 1，正向 -1
+}
+
+
+def handle_mouse_event(jog_callback, stop_callback, gripper_callback):
+ """
+ 监听 6D 鼠标事件，并调用传入的回调函数控制机械臂
+ :param jog_callback: 运动回调函数 (coord_id, direction)
+ :param stop_callback: 停止运动回调函数 (coord_id)
+ :param gripper_callback: 夹爪开合回调函数 ()
+ """
+ pygame.init()
+ pygame.joystick.init()
+
+ if pygame.joystick.get_count() == 0:
+ print("未检测到 6D 鼠标设备！")
+ return
+
+ joystick = pygame.joystick.Joystick(0)
+ joystick.init()
+ print(f"已连接 6D 鼠标: {joystick.get_name()}")
+
+ active_axes = {} # 记录当前运动状态
+ home_active = False # 记录初始点运动状态
+
+ try:
+ while True:
+ for event in pygame.event.get():
+ # 处理轴运动
+ if event.type == pygame.JOYAXISMOTION:
+ axis_id = event.axis
+ value = event.value
+
+ if axis_id in AXIS_MAPPING:
+ coord_id = AXIS_MAPPING[axis_id]
+ pos_dir, neg_dir = DIRECTION_MAPPING[axis_id]
+
+ if value > THRESHOLD and active_axes.get(axis_id) != 1:
+ jog_callback(coord_id, pos_dir)
+ active_axes[axis_id] = 1
+
+ elif value < -THRESHOLD and active_axes.get(axis_id) != -1:
+ jog_callback(coord_id, neg_dir)
+ active_axes[axis_id] = -1
+
+ elif -DEAD_ZONE < value < DEAD_ZONE and axis_id in active_axes:
+ stop_callback(coord_id)
+ del active_axes[axis_id]
+
+ # 处理按键（按钮 0 回到初始点，按钮 1 控制夹爪）
+ elif event.type == pygame.JOYBUTTONDOWN:
+ if event.button == 0: # 按下按钮 0，回到初始点
+ home_callback()
+ home_active = True
+ elif event.button == 1: # 按下按钮 1，切换夹爪状态
+ gripper_callback()
+
+ elif event.type == pygame.JOYBUTTONUP:
+ if event.button == 0 and home_active: # 松开按钮 0，停止初始点运动
+ home_stop_callback()
+ home_active = False
+
+ time.sleep(0.01)
+ except KeyboardInterrupt:
+ print("监听结束")
+ pygame.quit()
+
+
+# 定义回调函数
+def jog_callback(coord_id, direction):
+ """触发机械臂JOG坐标运动"""
+ if direction != 1:
+ direction = 0
+ if coord_id in [1,2,3]:
+ mc.jog_coord(coord_id, direction, jog_speed, _async=True)
+ else:
+ model = [2, 1, 3]
+ model_dir = [0, 1, 1]
+ model_id = model[coord_id-4]
+ model_dire = direction ^ model_dir[coord_id-4]
+ mc.jog_rpy(model_id, model_dire, jog_speed, _async=True)
+
+def stop_callback(coord_id):
+ """停止机械臂运动"""
+ mc.stop()
+
+def gripper_callback():
+ """控制夹爪开合"""
+ global gripper_state
+ gripper_state = not gripper_state
+ flag = 1 if gripper_state else 0
+ mc.set_gripper_state(flag, gripper_speed)
+
+def home_callback():
+ """回到初始点"""
+ mc.send_angles([0, 0, 0, -90, 0, 90, 0], home_speed, _async=True)
+
+def home_stop_callback():
+ """停止回到初始点"""
+ mc.stop()
+
+
+if __name__ == '__main__':
+ # 启动监听
+ handle_mouse_event(jog_callback, stop_callback, gripper_callback)
diff --git a/demo/Mercury_A1_demo/README.md b/demo/Mercury_A1_demo/README.md
new file mode 100644
index 0000000..5dc0caa
--- /dev/null
+++ b/demo/Mercury_A1_demo/README.md
@@ -0,0 +1,145 @@
+# 6D 鼠标控制
+
+**使用设备：** 3Dconnexion SpaceMouse Wireless 蓝牙无线版、蓝牙USB接收器。
+
+**适用机型：** Mercury A1。
+
+**末端执行器：** MyCobot Pro 自适应夹爪
+
+## 摆放位置
+
+将 6D 鼠标的 `3Dconnexion` 标志面朝向使用者，并按如下方式摆放：
+
+
+
+## 按键功能对应
+
+
+
+控制器帽是 SpaceMouse Wireless 的核心。它的 Six-Degrees-of-Freedom (6DoF)传感器允许您推、拉、旋转或倾斜以平移、缩放和旋转。
+
+
+
+
+**1:** 前后平移；控制机械臂的X轴坐标，向前平移控制X轴正方向，向后平移控制X轴负方向。
+
+
+
+**2:** 左右平移；控制机械臂的Y轴坐标，左平移控制Y轴正方向，右平移控制Y轴负方向。
+
+
+
+**3:** 上拉下压；控制机械臂的Z轴坐标，向上拉控制Z轴正方向，向下压控制Z轴负方向
+
+
+
+**4:** 左右倾斜；控制机械臂的翻滚角。
+
+
+
+**5:** 前后倾斜；控制机械臂的俯仰角。
+
+
+
+**6:** 左右旋转；控制机械臂的偏航角。
+
+
+
+- **按钮1：** 左侧按钮，长按按钮，机械臂移动到初始点位，松开按钮停止机械臂移动。
+
+- **按钮2：** 右侧按钮，点击按钮，控制夹爪的张开或者闭合。
+
+## 使用方式
+
+>> 注意：使用之前，请打开鼠标的电源开关。
+
+### 安装依赖
+
+```python
+pip install pygame
+```
+
+下载代码: https://github.com/elephantrobotics/pymycobot
+
+**这里使用机械臂的通信方式有串口控制和socket控制两种，将鼠标的蓝牙接收器连接到电脑或者机器系统。**
+
+### 方式1：串口控制
+
+打开终端，切换路径到 `pymycobot/demo/Mercury_A1_demo` 文件夹，运行程序即可：
+
+```python
+python3 6D_mouse_serial_serial_port_control.py
+```
+
+**注意：程序启动后，首先要长按左侧按钮（按钮1）将机械臂移动到预设的初始点位，然后再进行其他操作。**
+
+### 方式2：Socket控制
+
+>> 注意： raspberryPi版本 仅支持python3 使用此类前提的机械臂有服务器，并且已经开启服务。
+
+
+#### 启动服务端
+
+使用socket控制之前，需要注意：
+
+- 机械臂系统和控制端（客户端）需要在同一网络。
+
+- 需要先在机械臂系统里执行服务器文件，开启服务端。
+
+- 服务器文件执行后，提示“Binding succeeded” 和 “waiting connect” 表示开启成功。
+
+打开终端， 切换路径到`pymycobot/demo` 文件夹，运行程序即可：
+
+```python
+python3 server_A1_close_loop.py
+```
+
+#### 客户端
+
+**修改IP地址和端口号**
+
+在PC电脑端，切换路径到 `pymycobot/demo/Mercury_A1_demo` 文件夹，编辑6D_mouse_serial_socket_control.py文件：
+
+根据服务端真实的IP和端口号进行修改即可。
+
+```bash
+import pygame
+import time
+from pymycobot import MercurySocket
+
+# 初始化机械臂,IP和端口号需根据实际进行修改
+mc = MercurySocket('192.168.1.4', 9000)
+
+···
+```
+
+然后运行程序即可。
+
+```python
+python3 6D_mouse_serial_socket_control.py
+```
+
+**注意：程序启动后，首先要长按左侧按钮（按钮1）将机械臂移动到预设的初始点位，然后再进行其他操作。**
+
+### 修改速度
+
+可根据需要自行修改机械臂的JOG坐标运动速度、夹爪的速度和初始点运动速度。在文件的开头进行修改，具体如下：
+
+```bash
+···
+# jog 坐标运动速度
+jog_speed = 20
+
+# 夹爪速度
+gripper_speed = 70
+
+# 初始点运动速度
+home_speed = 10
+···
+```
+
+## 注意事项
+
+当 SpaceMouse Wireless 的状态 LED 变为红色时，说明它的电池电量不足 10%，此时应该给它
+充电。使用随附的 USB cable将 SpaceMouse 连接到计算机的充电端口。当 SpaceMouse Wireless 正
+在充电时，状态 LED 就会呈绿色并闪烁，充满电后则会变成长亮的绿色。
diff --git a/demo/Mercury_A1_demo/res/6d_mouse.jpg b/demo/Mercury_A1_demo/res/6d_mouse.jpg
new file mode 100644
index 0000000..064f1ed
Binary files /dev/null and b/demo/Mercury_A1_demo/res/6d_mouse.jpg differ
diff --git a/demo/Mercury_A1_demo/res/6d_mouse_button_control.png b/demo/Mercury_A1_demo/res/6d_mouse_button_control.png
new file mode 100644
index 0000000..43be5ea
Binary files /dev/null and b/demo/Mercury_A1_demo/res/6d_mouse_button_control.png differ
diff --git a/demo/Mercury_A1_demo/res/6d_mouse_control.jpg b/demo/Mercury_A1_demo/res/6d_mouse_control.jpg
new file mode 100644
index 0000000..8fb82a1
Binary files /dev/null and b/demo/Mercury_A1_demo/res/6d_mouse_control.jpg differ
diff --git a/demo/Mercury_A1_demo/res/6d_mouse_on_off.png b/demo/Mercury_A1_demo/res/6d_mouse_on_off.png
new file mode 100644
index 0000000..91c74f0
Binary files /dev/null and b/demo/Mercury_A1_demo/res/6d_mouse_on_off.png differ
diff --git a/demo/Mercury_A1_demo/res/forward_backward.gif b/demo/Mercury_A1_demo/res/forward_backward.gif
new file mode 100644
index 0000000..aeba6c5
Binary files /dev/null and b/demo/Mercury_A1_demo/res/forward_backward.gif differ
diff --git a/demo/Mercury_A1_demo/res/left_right.gif b/demo/Mercury_A1_demo/res/left_right.gif
new file mode 100644
index 0000000..c0c4566
Binary files /dev/null and b/demo/Mercury_A1_demo/res/left_right.gif differ
diff --git a/demo/Mercury_A1_demo/res/pitch.gif b/demo/Mercury_A1_demo/res/pitch.gif
new file mode 100644
index 0000000..0ec319c
Binary files /dev/null and b/demo/Mercury_A1_demo/res/pitch.gif differ
diff --git a/demo/Mercury_A1_demo/res/roll.gif b/demo/Mercury_A1_demo/res/roll.gif
new file mode 100644
index 0000000..cff1cac
Binary files /dev/null and b/demo/Mercury_A1_demo/res/roll.gif differ
diff --git a/demo/Mercury_A1_demo/res/up_down.gif b/demo/Mercury_A1_demo/res/up_down.gif
new file mode 100644
index 0000000..1fff8e4
Binary files /dev/null and b/demo/Mercury_A1_demo/res/up_down.gif differ
diff --git a/demo/Mercury_A1_demo/res/yaw.gif b/demo/Mercury_A1_demo/res/yaw.gif
new file mode 100644
index 0000000..35e0b20
Binary files /dev/null and b/demo/Mercury_A1_demo/res/yaw.gif differ
diff --git a/demo/Server_280_RDKX5.py b/demo/Server_280_RDKX5.py
new file mode 100644
index 0000000..b6e9ff4
--- /dev/null
+++ b/demo/Server_280_RDKX5.py
@@ -0,0 +1,202 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+import fcntl
+import logging
+import traceback
+import socket
+import struct
+import serial
+import Hobot.GPIO as GPIO
+
+GPIO.setwarnings(False)
+
+
+class GPIOProtocolCode:
+ SETUP_GPIO_MODE = 0xAA
+ SETUP_GPIO_STATE = 0xAB
+ SET_GPIO_OUTPUT = 0xAC
+ GET_GPIO_INPUT = 0xAD
+
+
+def to_string(data: bytes):
+ return ' '.join(map(lambda x: f'{x:02x}', data))
+
+
+def get_local_host(name: str):
+ host = None
+ dgram_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ try:
+ pack_res = struct.pack('256s', bytes(name, encoding="utf8"))
+ host = socket.inet_ntoa(fcntl.ioctl(dgram_socket.fileno(), 0x8915, pack_res)[20:24])
+ except Exception as e:
+ print(e)
+ finally:
+ dgram_socket.close()
+ return host
+
+
+localhost = get_local_host("wlan0")
+
+
+class SocketTransport(object):
+ def __init__(self, host="0.0.0.0", port=30002):
+ self.port = port
+ self.host = host
+ self.running = True
+ self.log = logging.getLogger("socket")
+ self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ self.socket.bind((host, port))
+ self.socket.listen(5)
+ if host == "0.0.0.0":
+ host = localhost
+ self.log.info(f"start listening on {host}:{port}")
+
+ def accept(self):
+ while self.running is True:
+ yield self.socket.accept()
+
+ def context(self, conn, buffer_size=1024):
+ while self.running is True:
+ try:
+ data_buffer = conn.recv(buffer_size)
+ if len(data_buffer) == 0:
+ break
+ yield data_buffer
+ except Exception as e:
+ self.log.error(f"error while reading socket: {e}")
+ traceback.print_exc()
+ break
+
+ def close(self):
+ self.log.info(f"close socket on {self.host}:{self.port}")
+ self.running = False
+ self.socket.close()
+
+
+class SerialTransport(object):
+ def __init__(self, comport="/dev/ttyS1", baudrate=100_0000, timeout=None):
+ self.serial = serial.Serial(port=comport, baudrate=baudrate, timeout=timeout)
+ self.log = logging.getLogger("serial")
+ self.baudrate = baudrate
+ self.comport = comport
+ self.open()
+ self.log.info(f"start serial on [{self.comport}] with baudrate [{self.baudrate}]")
+
+ def send(self, data):
+ self.serial.write(data)
+
+ def recv(self, size=1024):
+ return self.serial.read(size)
+
+ @property
+ def is_open(self):
+ return self.serial.is_open
+
+ def close(self):
+ self.serial.close()
+
+ def open(self):
+ if not self.serial.is_open:
+ self.serial.open()
+
+
+class MyCobot280RDKX5Server(object):
+ """
+ Server for 280 RDK-X5
+
+ 1. System GPIO operating protocol adheres to protocol MyCobot 280 RDK X5.
+ 2. This server only does the work of forwarding protocol and does not participate in the analysis of instructions.
+ 3. The server is only responsible for forwarding the data received from the socket to the serial port and vice versa.
+ 4. Instruction parsing is done entirely by the client
+ 5. The server is responsible for setting the GPIO mode
+ """
+ def __init__(self, socket_transport, serial_transport, debug=True):
+ self.debug = debug
+ self.socket_transport = socket_transport
+ self.serial_transport = serial_transport
+ self.log = logging.getLogger("server")
+
+ def mainloop(self):
+ try:
+ self.log.info("tcp server started.")
+ for conn, addr in self.socket_transport.accept():
+ self.log.info(f"{addr} accepted!")
+ for data in self.socket_transport.context(conn, buffer_size=1024):
+ self.serial_transport.log.info(f"{addr} recv << {to_string(data)}")
+ if data[3] == GPIOProtocolCode.SETUP_GPIO_MODE:
+ try:
+ mode = GPIO.BCM if data[4] == 0 else GPIO.BOARD
+ GPIO.setmode(mode)
+ self.log.debug(f"{addr} setup gpio mode => {mode}")
+ serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0x01, 0xfa])
+ except Exception as e:
+ self.log.error(f"{addr} setup gpio mode error: {e}")
+ serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0xff, 0xfa])
+
+ elif data[3] == GPIOProtocolCode.SETUP_GPIO_STATE:
+ try:
+ mode = GPIO.OUT if data[5] == 1 else GPIO.IN
+ level = GPIO.HIGH if data[6] == 1 else GPIO.LOW
+ self.log.debug(f"{addr} setup gpio state, mode => {mode}, level => {level}")
+ GPIO.setup(data[4], mode, initial=level)
+ serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0x01, 0xfa])
+ except Exception as e:
+ self.log.error(f"{addr} setup gpio state error: {e}")
+ serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0xff, 0xfa])
+
+ elif data[3] == GPIOProtocolCode.SET_GPIO_OUTPUT:
+ try:
+ level = GPIO.HIGH if data[5] == 1 else GPIO.LOW
+ self.log.debug(f"{addr} set gpio output, level => {level}")
+ GPIO.output(data[4], level)
+ serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0x01, 0xfa])
+ except Exception as e:
+ self.log.error(f"{addr} set gpio output error: {e}")
+ serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0xff, 0xfa])
+
+ elif data[3] == GPIOProtocolCode.GET_GPIO_INPUT:
+ try:
+ self.log.debug(f"{addr} get gpio input, channel => {data[4]}")
+ level = GPIO.input(data[4])
+ self.log.debug(f"{addr} get gpio input, level => {level}")
+ serial_data = bytes([0xfe, 0xfe, 0x03, data[3], level, 0xfa])
+ except Exception as e:
+ self.log.error(f"{addr} get gpio input error: {e}")
+ serial_data = bytes([0xfe, 0xfe, 0x03, data[3], 0xff, 0xfa])
+ else:
+ self.serial_transport.send(data)
+ serial_data = self.serial_transport.recv()
+
+ self.serial_transport.log.info(f"{addr} send >> {to_string(serial_data)}")
+ conn.send(serial_data)
+ else:
+ self.log.info(f"{addr} closed!")
+ except Exception as e:
+ self.log.error(f"server error: {e}")
+ self.log.exception(traceback.format_exc())
+ finally:
+ self.socket_transport.close()
+ self.serial_transport.close()
+ self.log.info("server closed")
+
+
+def main(debug=False):
+ logging.basicConfig(
+ level=logging.DEBUG if debug else logging.INFO,
+ format="%(asctime)s - [%(name)s] %(levelname)7s - %(message)s",
+ handlers=[
+ logging.StreamHandler(),
+ logging.FileHandler("server.log")
+ ]
+ )
+
+ socket_transport = SocketTransport(host="0.0.0.0", port=30002)
+
+ serial_transport = SerialTransport(comport="/dev/ttyS1", baudrate=100_0000, timeout=0.1)
+
+ MyCobot280RDKX5Server(socket_transport, serial_transport).mainloop()
+ GPIO.cleanup()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/demo/Server_A1.py b/demo/Server_A1.py
index 0dda27f..87614b5 100644
--- a/demo/Server_A1.py
+++ b/demo/Server_A1.py
@@ -63,6 +63,7 @@ def __init__(self, host, port, serial_num="/dev/ttyAMA1", baud=115200):
self.serial_num = serial_num
self.baud = baud
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ self.s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
self.s.bind((host, port))
print("Binding succeeded!")
self.s.listen(1)
@@ -98,13 +99,15 @@ def connect(self):
"return datas: {}".format([hex(v) for v in res]))
conn.sendall(res)
+ except ConnectionResetError:
+ print("close disconnect!")
+ time.sleep(0.1)
except Exception as e:
self.logger.error(traceback.format_exc())
- conn.sendall(str.encode(traceback.format_exc()))
break
except Exception as e:
self.logger.error(traceback.format_exc())
- conn.close()
+ self.conn.close()
self.mc.close()
def _encode_int16(self, data):
diff --git a/demo/Server_A1_2.py b/demo/Server_A1_2.py
new file mode 100644
index 0000000..11e7c7c
--- /dev/null
+++ b/demo/Server_A1_2.py
@@ -0,0 +1,498 @@
+#!/usr/bin/env python3
+# coding:utf-8
+import socket
+import serial
+import time
+import logging
+import logging.handlers
+import re
+import fcntl
+import struct
+import traceback
+import threading
+
+"""
+Instructions for use:
+
+Please update pymycobot to the latest version before use.
+
+`pip install pymycobot --upgrade`
+
+Please change the parameters passed in the last line of the Server.py file, MercuryServer, based on your model.
+
+
+"""
+class ProtocolCode(object):
+ # BASIC
+ HEADER = 0xFE
+ FOOTER = 0xFA
+
+ # System status
+ ROBOT_VERSION = 0x01
+ SOFTWARE_VERSION = 0x02
+ GET_ROBOT_ID = 0x03
+ OVER_LIMIT_RETURN_ZERO = 0x04
+ SET_ROBOT_ID = 0x04
+
+ GET_ERROR_INFO = 0x07
+ CLEAR_ERROR_INFO = 0x08
+ GET_ATOM_VERSION = 0x09
+
+ SET_CW = 0x0B
+ GET_CW = 0x0C
+ CLEAR_WAIST_QUEUE = 0x0D
+
+ SetHTSGripperTorque = 0x35
+ GetHTSGripperTorque = 0x36
+ GetGripperProtectCurrent = 0x37
+ InitGripper = 0x38
+ SetGripperProtectCurrent = 0x39
+
+ # Overall status
+ POWER_ON = 0x10
+ POWER_OFF = 0x11
+ IS_POWER_ON = 0x12
+ RELEASE_ALL_SERVOS = 0x13
+ IS_CONTROLLER_CONNECTED = 0x14
+ READ_NEXT_ERROR = 0x15
+ SET_FRESH_MODE = 0x16
+ GET_FRESH_MODE = 0x17
+ SET_FREE_MODE = 0x1A
+ IS_FREE_MODE = 0x1B
+ COBOTX_GET_ANGLE = 0x1C
+ POWER_ON_ONLY = 0x1D
+ SET_CONTROL_MODE = 0x1E
+ GET_CONTROL_MODE = 0x1F
+ FOCUS_ALL_SERVOS = 0x18
+ GO_ZERO = 0x19
+ SET_BREAK = 0x19
+
+ # MDI MODE AND OPERATION
+ GET_ANGLES = 0x20
+ SEND_ANGLE = 0x21
+ SEND_ANGLES = 0x22
+ GET_COORDS = 0x23
+ SEND_COORD = 0x24
+ SEND_COORDS = 0x25
+ PAUSE = 0x26
+ IS_PAUSED = 0x27
+ RESUME = 0x28
+ STOP = 0x29
+ IS_IN_POSITION = 0x2A
+ IS_MOVING = 0x2B
+ GET_ANGLE = 0x2C
+ GET_COORD = 0x2D
+ SEND_ANGLES_AUTO = 0x2E
+ GET_SOLUTION_ANGLES = 0x2E
+ SET_SOLUTION_ANGLES = 0x2F
+
+ # JOG MODE AND OPERATION
+ JOG_ANGLE = 0x30
+ JOG_ABSOLUTE = 0x31
+ JOG_COORD = 0x32
+ JOG_INCREMENT = 0x33
+ JOG_STOP = 0x34
+ JOG_INCREMENT_COORD = 0x34
+
+ COBOTX_GET_SOLUTION_ANGLES = 0x35
+ COBOTX_SET_SOLUTION_ANGLES = 0x36
+
+ SET_ENCODER = 0x3A
+ GET_ENCODER = 0x3B
+ SET_ENCODERS = 0x3C
+ GET_ENCODERS = 0x3D
+ SET_ENCODERS_DRAG = 0x3E
+
+ # RUNNING STATUS AND SETTINGS
+ GET_SPEED = 0x40
+ SET_SPEED = 0x41
+ GET_FEED_OVERRIDE = 0x42
+ GET_ACCELERATION = 0x44
+ SET_ACCELERATION = 0x45
+ GET_JOINT_MIN_ANGLE = 0x4A
+ GET_JOINT_MAX_ANGLE = 0x4B
+ SET_JOINT_MIN = 0x4C
+ SET_JOINT_MAX = 0x4D
+
+ # SERVO CONTROL
+ IS_SERVO_ENABLE = 0x50
+ IS_ALL_SERVO_ENABLE = 0x51
+ SET_SERVO_DATA = 0x52
+ GET_SERVO_DATA = 0x53
+ SET_SERVO_CALIBRATION = 0x54
+ JOINT_BRAKE = 0x55
+ RELEASE_SERVO = 0x56
+ FOCUS_SERVO = 0x57
+ SET_GRIPPER_ENABLED = 0x58
+ GET_ZERO_POS = 0x59
+ IS_INIT_CALIBRATION = 0x5A
+
+
+ # ATOM IO
+ SET_PIN_MODE = 0x60
+ SET_DIGITAL_OUTPUT = 0x61
+ GET_DIGITAL_INPUT = 0x62
+ SET_PWM_MODE = 0x63
+ SET_PWM_OUTPUT = 0x64
+ GET_GRIPPER_VALUE = 0x65
+ SET_GRIPPER_STATE = 0x66
+ SET_GRIPPER_VALUE = 0x67
+ SET_GRIPPER_CALIBRATION = 0x68
+ IS_GRIPPER_MOVING = 0x69
+ SET_COLOR = 0x6A
+ SET_GRIPPER_TORQUE = 0x6F
+ IS_BTN_CLICKED = 0x6F
+ SET_COLOR_MYARM = 0x70
+ SET_ELECTRIC_GRIPPER = 0x6B
+ INIT_ELECTRIC_GRIPPER = 0x6C
+ SET_GRIPPER_MODE = 0x6D
+ GET_GRIPPER_MODE = 0x6E
+
+ GET_ACCEI_DATA = 0x73
+ SET_COLLISION_MODE = 0x74
+ SET_COLLISION_THRESHOLD = 0x75
+ GET_COLLISION_THRESHOLD = 0x76
+ SET_TORQUE_COMP = 0x77
+ GET_TORQUE_COMP = 0x78
+ GET_VR_MODE = 0x79
+ SET_VR_MODE = 0x7A
+ GET_MODEL_DIRECTION = 0x7C
+ SET_MODEL_DIRECTION = 0x7D
+ GET_FILTER_LEN = 0x7E
+ SET_FILTER_LEN = 0x7F
+
+
+ # Basic
+ SET_BASIC_OUTPUT = 0xA0
+ GET_BASIC_INPUT = 0xA1
+ GET_BASE_INPUT = 0xA2
+ MERCURY_ROBOT_STATUS = 0xA2
+ MERCURY_ERROR_COUNTS = 0xA3
+ MERCURY_SET_POS_OVER_SHOOT = 0xA4
+ MERCURY_GET_POS_OVER_SHOOT = 0xA5
+ SET_BASE_PWM = 0xA5
+
+ # Linux GPIO, mode: GPIO.BCM
+ SET_GPIO_MODE = 0xAA
+ SET_GPIO_UP = 0xAB
+ SET_GPIO_OUTPUT = 0xAC
+ GET_GPIO_IN = 0xAD
+
+ # set WIFI
+ SET_SSID_PWD = 0xB0
+ GET_SSID_PWD = 0xB1
+ TOOL_SERIAL_RESTORE = 0xB1
+ TOOL_SERIAL_READY = 0xB2
+ TOOL_SERIAL_AVAILABLE = 0xB3
+ TOOL_SERIAL_READ_DATA = 0xB4
+ TOOL_SERIAL_WRITE_DATA = 0xB5
+ TOOL_SERIAL_FLUSH = 0xB6
+ TOOL_SERIAL_PEEK = 0xB7
+ TOOL_SERIAL_SET_BAUD = 0xB8
+ TOOL_SERIAL_SET_TIME_OUT = 0xB9
+ SET_SERVER_PORT = 0xB2
+
+ # Get the measured distance
+ GET_TOF_DISTANCE = 0xC0
+ GET_BASIC_VERSION = 0xC1
+ SET_COMMUNICATE_MODE = 0xC2
+ GET_COMMUNICATE_MODE = 0xC3
+
+ # Coordinate transformation
+ SET_TOOL_REFERENCE = 0x81
+ GET_TOOL_REFERENCE = 0x82
+ SET_WORLD_REFERENCE = 0x83
+ GET_WORLD_REFERENCE = 0x84
+ SET_REFERENCE_FRAME = 0x85
+ GET_REFERENCE_FRAME = 0x86
+ SET_MOVEMENT_TYPE = 0x87
+ GET_MOVEMENT_TYPE = 0x88
+ SET_END_TYPE = 0x89
+ GET_END_TYPE = 0x8A
+ WRITE_MOVE_C = 0x8C
+ SOLVE_INV_KINEMATICS = 0x8D
+
+ # Impact checking
+ SET_JOINT_CURRENT = 0x90
+ GET_JOINT_CURRENT = 0x91
+ SET_CURRENT_STATE = 0x92
+ GET_POS_OVER = 0x94
+ CLEAR_ENCODERS_ERROR = 0x95
+ GET_DOWN_ENCODERS = 0x96
+
+ # planning speed
+ GET_PLAN_SPEED = 0xD0
+ GET_PLAN_ACCELERATION = 0xD1
+ SET_PLAN_SPEED = 0xD2
+ SET_PLAN_ACCELERATION = 0xD3
+ move_round = 0xD4
+ GET_ANGLES_COORDS = 0xD5
+ GET_QUICK_INFO = 0xD6
+ SET_FOUR_PIECES_ZERO = 0xD7
+
+ # Motor status read
+ GET_SERVO_SPEED = 0xE1
+ GET_SERVO_CURRENTS = 0xE2
+ GET_SERVO_VOLTAGES = 0xE3
+ GET_SERVO_STATUS = 0xE4
+ GET_SERVO_TEMPS = 0xE5
+ GET_SERVO_LASTPDI = 0xE6
+ SERVO_RESTORE = 0xE7
+ SET_VOID_COMPENSATE = 0xE7
+ SET_ERROR_DETECT_MODE = 0xE8
+ GET_ERROR_DETECT_MODE = 0xE9
+
+ MERCURY_GET_BASE_COORDS = 0xF0
+ MERCURY_SET_BASE_COORD = 0xF1
+ MERCURY_SET_BASE_COORDS = 0xF2
+ MERCURY_JOG_BASE_COORD = 0xF3
+
+ MERCURY_DRAG_TECH_SAVE = 0x70
+ MERCURY_DRAG_TECH_EXECUTE = 0x71
+ MERCURY_DRAG_TECH_PAUSE = 0x72
+ MERCURY_DRAG_TEACH_CLEAN = 0x73
+
+ GET_ROBOT_MODIFIED_VERSION = 1
+ GET_ROBOT_FIRMWARE_VERSION = 2
+ GET_ROBOT_AUXILIARY_FIRMWARE_VERSION = 3
+ GET_ROBOT_ATOM_MODIFIED_VERSION = 4
+ GET_ROBOT_TOOL_FIRMWARE_VERSION = 9
+ GET_ROBOT_SERIAL_NUMBER = 5
+ SET_ROBOT_ERROR_CHECK_STATE = 6
+ GET_ROBOT_ERROR_CHECK_STATE = 7
+ GET_ROBOT_ERROR_STATUS = 0x15
+ GET_ATOM_PRESS_STATUS = 0x6b
+ GET_ATOM_LED_COLOR = 0x6a
+ SET_ATOM_PIN_STATUS = 0x61
+ GET_ATOM_PIN_STATUS = 0x62
+ SET_MASTER_PIN_STATUS = 0x65
+ GET_MASTER_PIN_STATUS = 0x66
+ SET_AUXILIARY_PIN_STATUS = 0xa0
+ GET_AUXILIARY_PIN_STATUS = 0xa1
+ SET_SERVO_MOTOR_CLOCKWISE = 0x73
+ GET_SERVO_MOTOR_CLOCKWISE = 0Xea
+ SET_SERVO_MOTOR_COUNTER_CLOCKWISE = 0x74
+ GET_SERVO_MOTOR_COUNTER_CLOCKWISE = 0xeb
+ SET_SERVO_MOTOR_CONFIG = 0x52
+ GET_SERVO_MOTOR_CONFIG = 0x53
+ CLEAR_RECV_QUEUE = 0x19
+ GET_RECV_QUEUE_LENGTH = 0x08
+ GET_BASE_COORDS = 0xF0
+ BASE_TO_SINGLE_COORDS = 0xF1
+ COLLISION = 0xF2
+ GET_BASE_COORD = 0xF3
+ GET_ALL_BASE_COORDS = 0xF4
+ WRITE_BASE_COORD = 0xF5
+ WRITE_BASE_COORDS = 0xF6
+ JOG_INC_COORD = 0xF7
+ COLLISION_SWITCH = 0xF8
+ IS_COLLISION_ON = 0xF9
+ CLEAR_ROBOT_ERROR = 0x16
+ GET_RECV_QUEUE_SIZE = 0x17
+ SET_RECV_QUEUE_SIZE = 0x18
+has_return = [0x02, 0x03, 0x04, 0x09, 0x10, 0x11, 0x12, 0x13, 0x1c, 0x18, 0x19, 0x20, 0x23, 0x27, 0x29, 0x2A, 0x2B, 0x35, 0x4A, 0x4B,0x4C, 0x4D,
+ 0x50, 0x51, 0x56,0x57, 0x59,0x5A,0x62, 0x82, 0x84, 0x86, 0x88, 0x8A, 0xA1, 0xA2, 0xB2, 0xB3, 0xB4, 0xB5, 0xB7, 0xD6, 0xe1, 0xe2, 0xe4]
+
+
+def get_logger(name):
+ logger = logging.getLogger(name)
+ logger.setLevel(logging.DEBUG)
+
+ LOG_FORMAT = "%(asctime)s - %(levelname)s - %(message)s"
+ # DATE_FORMAT = "%m/%d/%Y %H:%M:%S %p"
+
+ formatter = logging.Formatter(LOG_FORMAT)
+ console = logging.StreamHandler()
+ console.setFormatter(formatter)
+
+ save = logging.handlers.RotatingFileHandler(
+ "server.log", maxBytes=10485760, backupCount=1)
+ save.setFormatter(formatter)
+
+ logger.addHandler(save)
+ logger.addHandler(console)
+ return logger
+
+
+class MercuryServer(object):
+
+ def __init__(self, host, port, serial_num="/dev/ttyAMA1", baud=115200):
+ """Server class
+
+ Args:
+ host: server ip address.
+ port: server port.
+ serial_num: serial number of the robot.The default is /dev/ttyAMA1.
+ baud: baud rate of the serial port.The default is 115200.
+
+ """
+ self.logger = get_logger("AS")
+ self.mc = None
+ self.serial_num = serial_num
+ self.baud = baud
+ self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ self.s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ self.s.bind((host, port))
+ print("Binding succeeded!")
+ self.s.listen(1)
+ self.conn = None
+ self.stop = False
+ self.connected = False
+ self.mc = serial.Serial(self.serial_num, self.baud, timeout=0.1)
+ self.connect()
+
+ def connect(self):
+ while True:
+ try:
+ print("waiting connect!------------------")
+ self.conn, addr = self.s.accept()
+ # self.connected = True
+ while True:
+ try:
+ print("waiting data--------")
+ data = self.conn.recv(1024)
+ self.connected = True
+ command = []
+ for v in data:
+ command.append(v)
+ if command == []:
+ print("close disconnect!")
+ break
+ if self.mc.isOpen() == False:
+ self.mc.open()
+ else:
+ self.logger.info("get command: {}".format(
+ [hex(v) for v in command]))
+ # command = self.re_data_2(command)
+
+ self.write(command)
+ if command[3] == 0x29:
+ self.connected = False
+ if command[3] in has_return or (command[3] in [ProtocolCode.SEND_ANGLE, ProtocolCode.SEND_ANGLES, ProtocolCode.SEND_COORD, ProtocolCode.SEND_COORDS, ProtocolCode.JOG_INCREMENT, ProtocolCode.JOG_INCREMENT_COORD, ProtocolCode.COBOTX_SET_SOLUTION_ANGLES]):
+ # res = self.read(command)
+ self.read_thread = threading.Thread(target=self.read, args=(command,), daemon=True)
+ self.read_thread.start()
+ except ConnectionResetError:
+ self.connected = False
+ pass
+ except Exception as e:
+ self.logger.error(traceback.format_exc())
+ break
+ except Exception as e:
+ self.logger.error(traceback.format_exc())
+ self.conn.close()
+ self.mc.close()
+
+ def _encode_int16(self, data):
+ if isinstance(data, int):
+ return [
+ ord(i) if isinstance(i, str) else i
+ for i in list(struct.pack(">h", data))
+ ]
+ else:
+ res = []
+ for v in data:
+ t = self._encode_int16(v)
+ res.extend(t)
+ return res
+
+ @classmethod
+ def crc_check(cls, command):
+ crc = 0xffff
+ for index in range(len(command)):
+ crc ^= command[index]
+ for _ in range(8):
+ if crc & 1 == 1:
+ crc >>= 1
+ crc ^= 0xA001
+ else:
+ crc >>= 1
+ if crc > 0x7FFF:
+ return list(struct.pack(">H", crc))
+ return cls._encode_int16(_, crc)
+
+ def write(self, command):
+ self.mc.write(command)
+ self.mc.flush()
+
+ def read(self, command):
+ datas = b""
+ data_len = -1
+ k = 0
+ pre = 0
+ t = time.time()
+ wait_time = 0.1
+ if command[3] == 0x10:
+ wait_time = 8
+ elif command[3] in [0x11, 0x13, 0x18, 0x56, 0x57, 0x29]:
+ wait_time = 3
+ elif command[3] in [ProtocolCode.SEND_ANGLE, ProtocolCode.SEND_ANGLES, ProtocolCode.SEND_COORD, ProtocolCode.SEND_COORDS, ProtocolCode.JOG_INCREMENT, ProtocolCode.JOG_INCREMENT_COORD, ProtocolCode.COBOTX_SET_SOLUTION_ANGLES]:
+ wait_time = 300
+ while True and time.time() - t < wait_time and self.connected:
+ if self.mc.inWaiting() > 0:
+ data = self.mc.read()
+ # print("read data: ", data)
+ # if data != b"":
+ # print(data, datas)
+ k += 1
+ if data_len == 3:
+ datas += data
+ crc = self.mc.read(2)
+ if self.crc_check(datas) == [v for v in crc]:
+ datas+=crc
+ break
+ if data_len == 1 and data == b"\xfa":
+ datas += data
+ # if [i for i in datas] == command:
+ # datas = b''
+ # data_len = -1
+ # k = 0
+ # pre = 0
+ # continue
+ break
+ elif len(datas) == 2:
+ data_len = struct.unpack("b", data)[0]
+ datas += data
+ elif len(datas) > 2 and data_len > 0:
+ datas += data
+ data_len -= 1
+ # if len(datas) == 4:
+ # if datas[-1] != command[3]:
+ # datas = b''
+ # data_len = -1
+ # k = 0
+ # pre = 0
+ # continue
+ elif data == b"\xfe":
+ if datas == b"":
+ datas += data
+ pre = k
+ else:
+ if k - 1 == pre:
+ datas += data
+ else:
+ datas = b"\xfe"
+ pre = k
+ if self.conn is not None:
+ self.logger.info("return datas: {}".format([hex(v) for v in datas]))
+
+ self.conn.sendall(datas)
+ datas = b''
+ return datas
+
+ def re_data_2(self, command):
+ r2 = re.compile(r'[[](.*?)[]]')
+ data_str = re.findall(r2, command)[0]
+ data_list = data_str.split(",")
+ data_list = [int(i) for i in data_list]
+ return data_list
+
+
+if __name__ == "__main__":
+ ifname = "wlan0"
+ s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ HOST = socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, struct.pack(
+ '256s', bytes(ifname, encoding="utf8")))[20:24])
+ PORT = 9000
+ print("ip: {} port: {}".format(HOST, PORT))
+ MercuryServer(HOST, PORT, "/dev/ttyAMA1", 115200)
diff --git a/demo/Server_X1_close_loop.py b/demo/Server_X1_close_loop.py
new file mode 100644
index 0000000..26dbef9
--- /dev/null
+++ b/demo/Server_X1_close_loop.py
@@ -0,0 +1,343 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+import dataclasses
+import json
+import struct
+import fcntl
+import threading
+import traceback
+from typing import Union, Optional, List, Generator
+import serial
+import socket
+import time
+import logging
+from logging.handlers import RotatingFileHandler
+from concurrent.futures import ThreadPoolExecutor, Future
+"""
+This is a demo server for Mercury X1 robot arm. It can be used to control the robot arm via TCP/IP.
+"""
+
+
+def get_local_host(name: str = "eth0") -> Optional[str]:
+ host = None
+ dgram_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ try:
+ pack_res = struct.pack('256s', bytes(name, encoding="utf8"))
+ host = socket.inet_ntoa(fcntl.ioctl(dgram_socket.fileno(), 0x8915, pack_res)[20:24])
+ except Exception as e:
+ print(e)
+ finally:
+ dgram_socket.close()
+ return host
+
+
+def init_logging(name: str, level: int = logging.INFO):
+ logger = logging.getLogger(name)
+ logger.setLevel(level=level)
+ handle = RotatingFileHandler(f"{name}.log", maxBytes=1024 * 1024 * 5, backupCount=5, encoding="utf-8")
+ handle.setLevel(level=level)
+ logger.addHandler(handle)
+ return logger
+
+
+class Arm:
+ LEFT_ARM = "left_arm"
+ RIGHT_ARM = "right_arm"
+ UNKNOWN = "unknown"
+
+
+@dataclasses.dataclass
+class Client:
+ arm: str
+ address: str
+ socket: socket.socket
+
+ def receive(self) -> Optional[dict]:
+ message = None
+ buffer_prefix = self.socket.recv(4) # Let's assume that each message length prefix occupies 4 bytes
+ if buffer_prefix:
+ buffer_size = struct.unpack('!I', buffer_prefix)[0]
+ data_bytes = self.socket.recv(buffer_size)
+ message = json.loads(data_bytes.decode('utf-8'))
+ return message
+
+ def send(self, message: dict):
+ message_bytes = json.dumps(message).encode()
+ buffer_size = struct.pack('!I', len(message_bytes))
+ self.socket.sendall(buffer_size + message_bytes)
+
+ def raw_send(self, data: bytes):
+ self.socket.sendto(data, self.address)
+
+ @property
+ def host(self):
+ return "{}:{}".format(*self.address)
+
+
+class MercurySerialApi(object):
+
+ def __init__(self, comport: str, baudrate: int, timeout: float = 1, debug: bool = False):
+ level = logging.DEBUG if debug else logging.INFO
+ self.comport = comport
+ self.baudrate = baudrate
+ self.timeout = timeout
+ self.mutex = threading.Lock()
+ self.log = init_logging("server.serial", level)
+ self.serial = serial.Serial(self.comport, self.baudrate, timeout=self.timeout)
+
+ def open(self):
+ if self.serial.is_open:
+ self.log.debug(f"Serial port {self.comport} is already opened")
+ return
+ self.serial.open()
+ self.log.debug(f"Open serial port {self.comport} with baudrate {self.baudrate}")
+
+ def close(self):
+ self.serial.close()
+ self.log.debug(f"Close serial port {self.comport}")
+
+ def send(self, data: Union[bytes, List[int]]):
+ self.serial.write(data)
+ self.log.info(f"[{self.comport}] write: {data}")
+
+ def _encode_int16(self, data):
+ if isinstance(data, int):
+ return [ord(i) if isinstance(i, str) else i for i in list(struct.pack(">h", data))]
+
+ res = []
+ for v in data:
+ res.extend(self._encode_int16(v))
+ return res
+
+ @classmethod
+ def _crc_check(cls, commands: bytes) -> Optional[List[int]]:
+ crc = 0xffff
+ for command in commands:
+ crc ^= command
+ for _ in range(8):
+ if crc & 1 == 1:
+ crc >>= 1
+ crc ^= 0xA001
+ else:
+ crc >>= 1
+ if crc > 0x7FFF:
+ return list(struct.pack(">H", crc))
+
+ @classmethod
+ def _decode_int16(cls, data):
+ return struct.unpack(">h", data)[0]
+
+ def read(self, timeout: float = 0.5) -> bytes:
+ data_len = -1
+ k = 0
+ pre = 0
+ datas = b""
+ stime = time.time()
+ while time.time() - stime < timeout:
+
+ if self.serial.is_open and self.serial.in_waiting > 0:
+ data = self.serial.read()
+ k += 1
+ if data_len == 3:
+ datas += data
+ crc = self.serial.read(2)
+ if self._crc_check(datas) == [v for v in crc]:
+ datas += crc
+ break
+
+ if data_len == 1 and data == b"\xfa":
+ datas += data
+ elif len(datas) == 2:
+ data_len = struct.unpack("b", data)[0]
+ datas += data
+ elif len(datas) > 2 and data_len > 0:
+ datas += data
+ data_len -= 1
+ elif data == b"\xfe":
+ if datas == b"":
+ datas += data
+ pre = k
+ else:
+ if k - 1 == pre:
+ datas += data
+ else:
+ datas = b"\xfe"
+ pre = k
+ else:
+ time.sleep(0.001)
+ return datas
+
+
+class MercurySerialManager(object):
+
+ def __init__(self):
+ self.__mian_arm = Arm.RIGHT_ARM
+ self.__arm_serial_map: Union[str:Optional[MercurySerialApi]] = {}
+
+ def __setitem__(self, key: str, value: MercurySerialApi):
+ self.__arm_serial_map[key] = value
+
+ def __getitem__(self, key: str) -> Optional[MercurySerialApi]:
+ return self.__arm_serial_map[key]
+
+ @property
+ def arms(self) -> List[str]:
+ return list(self.__arm_serial_map.keys())
+
+ def add_arm(self, arm_name: str, serial_api: MercurySerialApi):
+ self.__arm_serial_map[arm_name] = serial_api
+
+ def get_arm(self, arm_name: str) -> Optional[MercurySerialApi]:
+ return self.__arm_serial_map.get(arm_name, None)
+
+ @property
+ def main_arm(self) -> Optional[MercurySerialApi]:
+ return self.get_arm(self.__mian_arm)
+
+ @main_arm.setter
+ def main_arm(self, arm_name: str):
+ assert arm_name in self.__arm_serial_map, f"Arm {arm_name} is not exist"
+ self.__mian_arm = arm_name
+
+
+class MercurySocketInlet(object):
+
+ def __init__(self, host: str, port: int, debug: bool = False):
+ level = logging.DEBUG if debug else logging.INFO
+ self.host = host
+ self.port = port
+ self.log = init_logging("server.socket", level)
+ self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ self.socket.bind((self.host, self.port))
+ self.socket.listen(5)
+ self.log.info(f"Start listening on {self.host}:{self.port}")
+
+ def accept(self):
+ while True:
+ yield self.socket.accept()
+
+ def close(self):
+ self.socket.close()
+ self.log.info(f"Close socket on {self.host}:{self.port}")
+
+
+class MercuryCommandServer(object):
+
+ def __init__(self, serial_manager: MercurySerialManager, socket_inlet: MercurySocketInlet, debug: bool = False):
+ level = logging.DEBUG if debug else logging.INFO
+ self.log = init_logging("server.command", level)
+ self.serial_manager = serial_manager
+ self.socket_inlet = socket_inlet
+ self.connected_client_list: List[Client] = []
+ self.respond_client_threads = []
+ self.max_thread_pool = 5
+
+ def robot_serial_data_reader(self, arm: str, serial_api: MercurySerialApi):
+ serial_api.open()
+ while True:
+ data = serial_api.read()
+ if not data:
+ time.sleep(0.001)
+ continue
+
+ self.log.info(f"[{serial_api.comport}] read : {list(data)}")
+ for client in self.connected_client_list:
+ if client.arm != arm:
+ continue
+ self.log.info(" * send data to client")
+ client.raw_send(data)
+
+ def accept_client_connection(self) -> Generator[Client, None, None]:
+ """Accept client connection and return a client object"""
+ for client_socket, client_address in self.socket_inlet.accept():
+ if len(self.connected_client_list) >= self.max_thread_pool:
+ self.log.warning(f"too many clients, reject connection from {client_address}")
+ client_socket.close()
+ continue
+
+ self.log.info(f" * connection from {client_address}")
+ yield Client(arm=Arm.UNKNOWN, address=client_address, socket=client_socket)
+
+ def __on_future_done(self, future: Future):
+ client: Client = future.result()
+ if client in self.connected_client_list:
+ client.socket.close()
+ self.log.info(f" * disconnect from {client.address}")
+ self.connected_client_list.remove(client)
+
+ def __client_request_handle(self, client: Client) -> Client:
+ while True:
+ try:
+ message = client.receive()
+ if message is not None:
+ arm = message.get("arm", Arm.UNKNOWN)
+ commands: List[int] = message.get("command", [])
+ if client.arm == Arm.UNKNOWN and arm != Arm.UNKNOWN:
+ client.arm = arm
+
+ self.log.info(f" [{client.address}] -> {commands}")
+ self.serial_manager[arm].send(commands)
+ except ConnectionResetError:
+ break
+
+ except Exception as e:
+ self.log.error(f"Error occurred while handling client {client.host}: {e}")
+ self.log.error(traceback.format_exc())
+ return client # must be returned
+
+ def enter_mian_loop(self):
+ try:
+ self.log.info("Start command server")
+ for arm_name in self.serial_manager.arms:
+ serial_api = self.serial_manager.get_arm(arm_name)
+ if serial_api is None:
+ self.log.error(f"Arm {arm_name} not found")
+ continue
+
+ thread = threading.Thread(target=self.robot_serial_data_reader, args=(arm_name, serial_api),
+ daemon=True)
+ self.respond_client_threads.append(thread)
+
+ for thread in self.respond_client_threads:
+ thread.start()
+
+ with ThreadPoolExecutor(max_workers=self.max_thread_pool) as executor:
+ for client in self.accept_client_connection():
+ self.log.info(f" * accept connection from {client.address}")
+ future: Future = executor.submit(self.__client_request_handle, client)
+ future.add_done_callback(self.__on_future_done)
+ self.connected_client_list.append(client)
+
+ for thread in self.respond_client_threads:
+ thread.join()
+ except KeyboardInterrupt:
+ pass
+ finally:
+ self.log.info("Stop command server")
+ self.socket_inlet.close()
+
+
+def main(host: str, port: int, debug: bool = False):
+ basic_level = logging.DEBUG if debug else logging.INFO
+ basic_format = '[%(levelname)8s] | [%(name)14s] %(asctime)s - %(message)s'
+ basic_date_format = '%Y-%m-%d %H:%M:%S'
+ logging.basicConfig(level=basic_level, format=basic_format, datefmt=basic_date_format)
+
+ serial_manager = MercurySerialManager()
+
+ left_arm = MercurySerialApi("/dev/left_arm", 115200, debug=debug)
+ right_arm = MercurySerialApi("/dev/right_arm", 115200, debug=debug)
+
+ serial_manager.add_arm(Arm.LEFT_ARM, left_arm)
+ serial_manager.add_arm(Arm.RIGHT_ARM, right_arm)
+
+ socket_inlet = MercurySocketInlet(host=host, port=port, debug=debug)
+
+ mercury_command = MercuryCommandServer(serial_manager=serial_manager, socket_inlet=socket_inlet, debug=debug)
+ mercury_command.enter_mian_loop()
+
+
+if __name__ == '__main__':
+ main(host="0.0.0.0", port=9000, debug=True)
+
+
diff --git a/demo/basic.py b/demo/basic.py
index f467858..48bbaa6 100644
--- a/demo/basic.py
+++ b/demo/basic.py
@@ -1,7 +1,7 @@
import time
import os
import sys
-from pymycobot.mycobot import MyCobot
+from pymycobot.mycobot280 import MyCobot280
from pymycobot.genre import Angle, Coord
sys.path.append(os.path.dirname(__file__))
diff --git a/demo/change_pid.py b/demo/change_pid.py
index ea1e70f..192db64 100644
--- a/demo/change_pid.py
+++ b/demo/change_pid.py
@@ -1,7 +1,7 @@
# coding: utf-8
import time
from turtle import goto
-from pymycobot.mycobot import MyCobot
+from pymycobot.mycobot280 import MyCobot280
import serial.tools.list_ports
data_id = [21, 22, 23, 24, 26, 27]
@@ -44,7 +44,7 @@ def setup(): #机械臂检测
for port in ports:
try:
- mycobot = MyCobot(port, baud)
+ mycobot = MyCobot280(port, baud)
except Exception as e:
print(e)
exit(1)
diff --git a/demo/drag_trial_teaching.py b/demo/drag_trial_teaching.py
index fde57c2..f11ea65 100644
--- a/demo/drag_trial_teaching.py
+++ b/demo/drag_trial_teaching.py
@@ -12,22 +12,24 @@
from pymycobot.mycobot320 import MyCobot320
from pymycobot.mecharm270 import MechArm270
from pymycobot.myarm import MyArm
-
+from pymycobot.mypalletizer260 import MyPalletizer260
port: str
mc: MyCobot280
sp: int = 80
+
def setup():
global port, mc
print("")
-
+
print("1 - MyCobot280")
print("2 - MyCobot320")
- print("3 - MechArm")
- print("4 - MyArm")
- _in = input("Please input 1 - 4 to choose:")
+ print("3 - MechArm270")
+ print("4 - MyArm300")
+ print("5 - MyPalletizer260")
+ _in = input("Please input 1 - 5 to choose:")
robot_model = None
if _in == "1":
robot_model = MyCobot280
@@ -45,24 +47,21 @@ def setup():
elif _in == "2":
robot_model = MyCobot320
print("MyCobot320\n")
- print("Please enter the model type:")
- print("1. Pi")
- print("2. Jetson Nano")
- print("Default is Pi")
- model_type = input()
- if model_type == "2":
- port = "/dev/ttyTHS1"
- else:
- pass
elif _in == "3":
robot_model = MechArm270
- print("MechArm\n")
+ print("MechArm270\n")
+
elif _in == "4":
robot_model = MyArm
- print("MyArm\n")
+ print("MyArm300\n")
+
+ elif _in == '5':
+ robot_model = MyPalletizer260
+ print('MyPalletizer260\n')
+
else:
- print("Please choose from 1 - 3.")
+ print("Please choose from 1 - 5.")
print("Exiting...")
exit()
@@ -72,9 +71,11 @@ def setup():
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()
+ if not plist:
+ pass
+ else:
+ _in = input("\nPlease input 1 - {} to choice:".format(idx - 1))
+ port = str(plist[int(_in) - 1]).split(" - ")[0].strip()
print(port)
print("")
@@ -91,7 +92,6 @@ def setup():
f = input("Wether DEBUG mode[Y/n](default:n):")
if f in ["y", "Y", "yes", "Yes"]:
DEBUG = True
- # mc = MyCobot(port, debug=True)
mc = robot_model(port, baud, debug=DEBUG)
mc.power_on()
@@ -134,25 +134,43 @@ def __init__(self, mycobot) -> None:
def record(self):
self.record_list = []
self.recording = True
- #self.mc.set_fresh_mode(0)
+
+ # self.mc.set_fresh_mode(0)
+ if isinstance(self.mc, MyCobot320):
+ self.mc.release_all_servos(1)
+ else:
+ self.mc.release_all_servos()
+
def _record():
while self.recording:
start_t = time.time()
- angles = self.mc.get_encoders()
- speeds = self.mc.get_servo_speeds()
- gripper_value = self.mc.get_encoder(7)
- interval_time = time.time() - start_t
- if angles and speeds:
- record = [angles, speeds, gripper_value, interval_time]
- self.record_list.append(record)
- # time.sleep(0.1)
- print("\r {}".format(time.time() - start_t), end="")
+ if isinstance(self.mc, (MyArm, MyPalletizer260)):
+ angles = self.mc.get_encoders()
+ interval_time = time.time() - start_t
+ if angles:
+ record = [angles, interval_time]
+ self.record_list.append(record)
+ print("\r {}".format(time.time() - start_t), end="")
+ else:
+ angles = self.mc.get_encoders()
+ speeds = self.mc.get_servo_speeds()
+ interval_time = time.time() - start_t
+ if angles and speeds:
+ record = [angles, speeds, interval_time]
+ self.record_list.append(record)
+ print("\r {}".format(time.time() - start_t), end="")
self.echo("Start recording.")
self.record_t = threading.Thread(target=_record, daemon=True)
self.record_t.start()
def stop_record(self):
+
+ if isinstance(self.mc, MyArm):
+ self.mc.power_on()
+ else:
+ self.mc.focus_all_servos()
+
if self.recording:
self.recording = False
self.record_t.join()
@@ -160,16 +178,20 @@ def stop_record(self):
def play(self):
self.echo("Start play")
- i = 0
- for record in self.record_list:
- angles, speeds, gripper_value, interval_time = record
- #print(angles)
- self.mc.set_encoders_drag(angles, speeds)
- self.mc.set_encoder(7, gripper_value, 80)
- if i == 0:
- time.sleep(3)
- i+=1
- time.sleep(interval_time)
+ if isinstance(self.mc, (MyArm, MyPalletizer260)):
+ for record in self.record_list:
+ encoders, interval_time = record
+ self.mc.set_encoders(encoders, 100)
+ time.sleep(interval_time)
+ else:
+ i = 0
+ for record in self.record_list:
+ encoders, speeds, interval_time = record
+ self.mc.set_encoders_drag(encoders, speeds)
+ if i == 0:
+ time.sleep(3)
+ i += 1
+ time.sleep(interval_time)
self.echo("Finish play")
def loop_play(self):
@@ -248,7 +270,10 @@ def start(self):
elif key == "l": # load from local
self.load_from_local()
elif key == "f": # free move
- self.mc.release_all_servos()
+ if isinstance(self.mc, MyCobot320):
+ self.mc.release_all_servos(1)
+ else:
+ self.mc.release_all_servos()
self.echo("Released")
else:
print(key)
diff --git a/demo/gripper.py b/demo/gripper.py
index e8d1856..914879b 100644
--- a/demo/gripper.py
+++ b/demo/gripper.py
@@ -1,7 +1,7 @@
import os
import time
import sys
-from pymycobot.mycobot import MyCobot
+from pymycobot import MyCobot280
sys.path.append(os.path.dirname(__file__))
from port_setup import setup
diff --git a/demo/loop.py b/demo/loop.py
index 38152b3..8e50a3e 100644
--- a/demo/loop.py
+++ b/demo/loop.py
@@ -1,7 +1,7 @@
import time
import os
import sys
-from pymycobot.mycobot import MyCobot
+from pymycobot import MyCobot280
sys.path.append(os.path.dirname(__file__))
from port_setup import setup
diff --git a/demo/myArm_M&C_demo/README.md b/demo/myArm_M&C_demo/README.md
index c6c3618..4a23ad2 100644
--- a/demo/myArm_M&C_demo/README.md
+++ b/demo/myArm_M&C_demo/README.md
@@ -1,19 +1,24 @@
-# 使用方法
+# myArm M&C control case
-1. 先将myArm M连接电脑
-2. 执行myarm_m.py，输入myarm_m端口号对应的序号
-3. 再将myArm C连接电脑
-4. 执行myarm_c.py，输入myarm_c端口号对应的序号
+[中文](./README_ZH.md)
-**注：** myarm_m.py包含服务器，需先于myarm_m.py启动
+## Installation dependency
-------------------------------------------------------------------------
+```shell
+pip install -r requirement.txt
+```
-# Usage
+## Run the program
-1. Connect myArm M to the computer first
-2. Execute myarm_m.py and enter the serial number corresponding to the myarm_m port number
-3. Then connect myArm C to the computer
-4. Execute myarm_c.py and enter the serial number corresponding to the myarm_c port number
+```shell
+python main.py
+```
-**Note:** myarm_m.py contains the server and must be started before myarm_m.py
\ No newline at end of file
+## Program instructions
+
+There is a sequence requirement for opening the serial port: first open the serial port connection of myArmM, then open the serial port connection of myArmC.
+
+
+
+
+After both serial ports are opened, you can control the movement of myArmM by moving myArmC.
\ No newline at end of file
diff --git a/demo/myArm_M&C_demo_v1.1/README.md b/demo/myArm_M&C_demo_v1.1/README.md
index c6c3618..4a23ad2 100644
--- a/demo/myArm_M&C_demo_v1.1/README.md
+++ b/demo/myArm_M&C_demo_v1.1/README.md
@@ -1,19 +1,24 @@
-# 使用方法
+# myArm M&C control case
-1. 先将myArm M连接电脑
-2. 执行myarm_m.py，输入myarm_m端口号对应的序号
-3. 再将myArm C连接电脑
-4. 执行myarm_c.py，输入myarm_c端口号对应的序号
+[中文](./README_ZH.md)
-**注：** myarm_m.py包含服务器，需先于myarm_m.py启动
+## Installation dependency
-------------------------------------------------------------------------
+```shell
+pip install -r requirement.txt
+```
-# Usage
+## Run the program
-1. Connect myArm M to the computer first
-2. Execute myarm_m.py and enter the serial number corresponding to the myarm_m port number
-3. Then connect myArm C to the computer
-4. Execute myarm_c.py and enter the serial number corresponding to the myarm_c port number
+```shell
+python main.py
+```
-**Note:** myarm_m.py contains the server and must be started before myarm_m.py
\ No newline at end of file
+## Program instructions
+
+There is a sequence requirement for opening the serial port: first open the serial port connection of myArmM, then open the serial port connection of myArmC.
+
+
+
+
+After both serial ports are opened, you can control the movement of myArmM by moving myArmC.
\ No newline at end of file
diff --git a/demo/myArm_M&C_demo_v1.1/get_date.py b/demo/myArm_M&C_demo_v1.1/get_date.py
index 4108b5f..329bb6d 100644
--- a/demo/myArm_M&C_demo_v1.1/get_date.py
+++ b/demo/myArm_M&C_demo_v1.1/get_date.py
@@ -10,9 +10,9 @@ class CreateSerial(QThread):
def __init__(self, port, index, parent_serial=None):
super().__init__()
if index in [1,2]:
- self.serial = MyArmC(port)
+ self.serial = MyArmC(port, 1000000)
else:
- self.serial = MyArmM(port)
+ self.serial = MyArmM(port, 1000000)
self.index = index
self.parent_serial = parent_serial
self.serial_type = self.serial.__class__.__name__
@@ -34,21 +34,20 @@ def run(self) -> None:
if (self.index == 1 or self.index == 2) and self.parent_serial is not None and data["angle"] and data["speed"]:
- data["angle"] = self.serial.get_servos_encoder()
- data["speed"] = self.serial.get_servos_speed()
- data["angle"][3] = 4096 - data["angle"][3]
- data["angle"][-1] *= 1.1
- data["angle"][-1] = int(data["angle"][-1])
- if data["angle"][-1] > 2048:
- data["angle"][-1] = 2048
- # self.parent_serial.set_servos_encoder_drag(data["angle"], data["speed"])
- self.parent_serial.serial.set_servos_encoder(data["angle"], 100)
+ data["angle"] = self.serial.get_joints_angle()
+ data["angle"][-1] = round((data["angle"][-1] - 0.08) / (-95.27 - 0.08) * (-123.13 + 1.23) - 1.23, 2)
+ if data["angle"][-1] > 2:
+ data["angle"][-1] = 2
+ elif data["angle"][-1] < -118:
+ data["angle"][-1] = -118
+ self.parent_serial.serial.set_joints_angle(data["angle"], 100)
else:
- data["angle"] = self.serial.get_servos_encoder()
- data["speed"] = self.serial.get_servos_speed()
+ # data["angle"] = self.serial.get_servos_encoder()
+ # data["speed"] = self.serial.get_servos_speed()
time.sleep(1)
- if data["angle"] and data["speed"]:
- self.progress.emit({str(self.index):data})
+ self.progress.emit({str(self.index):data})
+ # if data["angle"]:
+ # self.progress.emit({str(self.index):data})
except:
pass
time.sleep(0.0001)
\ No newline at end of file
diff --git a/demo/myArm_M&C_demo_v1.1/main.py b/demo/myArm_M&C_demo_v1.1/main.py
index 93359fa..0739707 100644
--- a/demo/myArm_M&C_demo_v1.1/main.py
+++ b/demo/myArm_M&C_demo_v1.1/main.py
@@ -165,8 +165,8 @@ def add_row(self, data):
# tableWidget = self.tableWidget_4
# j = "2"
# index = j
-
- for i in range(8):
+ # print(data[index])
+ for i in range(7):
angle = tableWidget.item(i, 1)
angle.setText(str(data[index]["angle"][i]))
speed = tableWidget.item(i, 2)
diff --git a/demo/pid_read_write.py b/demo/pid_read_write.py
index 746d897..f3a6314 100644
--- a/demo/pid_read_write.py
+++ b/demo/pid_read_write.py
@@ -1,20 +1,24 @@
-from pymycobot import MyCobot
+from pymycobot import MyCobot280, MyCobot320
# from pymycobot import PI_PORT
import time
import serial
# pi
# 280
-# mc = MyCobot(PI_PORT, 1000000)
-# mc = MyCobot('/dev/ttyAMA0', 1000000)
+# mc = MyCobot280(PI_PORT, 1000000)
+# mc = MyCobot280('/dev/ttyAMA0', 1000000)
# 320
-# mc = MyCobot(PI_PORT, 115200)
-# mc = MyCobot('/dev/ttyAMA0', 115200)
+# mc = MyCobot320(PI_PORT, 115200)
+# mc = MyCobot320('/dev/ttyAMA0', 115200)
# M5
-# 280/320
-mc = MyCobot('COM66', 115200)
-# mc = MyCobot('/dev/ttyUSB0',115200)
+# 280
+mc = MyCobot280('COM66', 115200)
+# mc = MyCobot280('/dev/ttyUSB0',115200)
+
+# 320
+# mc = MyCobot320('COM66', 115200)
+# mc = MyCobot320('/dev/ttyUSB0',115200)
# 参数对应地址
data_id = [7, 21, 22, 23, 24, 26, 27]
diff --git a/demo/port_setup.py b/demo/port_setup.py
index 7a06613..49dbc6c 100644
--- a/demo/port_setup.py
+++ b/demo/port_setup.py
@@ -1,7 +1,7 @@
import serial
import serial.tools.list_ports
-from pymycobot.mycobot import MyCobot
+from pymycobot.mycobot280 import MyCobot280
@@ -33,6 +33,6 @@ def setup():
if f in ["y", "Y", "yes", "Yes"]:
DEBUG = True
# mc = MyCobot(port, debug=True)
- mc = MyCobot(port, baud, debug=DEBUG)
+ mc = MyCobot280(port, baud, debug=DEBUG)
return mc
diff --git a/demo/reader.py b/demo/reader.py
index 804e6ff..35feb94 100644
--- a/demo/reader.py
+++ b/demo/reader.py
@@ -1,9 +1,14 @@
-from pymycobot.mycobot import MyCobot
+from pymycobot import MyCobot280, MyCobot320
from port_setup import setup
-mc: MyCobot
+# 280
+mc: MyCobot280
sp: int = 80
+# 320
+# mc: MyCobot320
+# sp: int = 80
+
def setup():
print("")
diff --git a/demo/server_A1_close_loop.py b/demo/server_A1_close_loop.py
new file mode 100644
index 0000000..211399d
--- /dev/null
+++ b/demo/server_A1_close_loop.py
@@ -0,0 +1,519 @@
+#!/usr/bin/env python3
+# coding:utf-8
+import socket
+import serial
+import time
+import logging
+import logging.handlers
+import re
+import fcntl
+import struct
+import traceback
+import threading
+
+"""
+Instructions for use:
+
+Please update pymycobot to the latest version before use.
+
+`pip install pymycobot --upgrade`
+
+Please change the parameters passed in the last line of the Server.py file, MercuryServer, based on your model.
+
+
+"""
+class ProtocolCode(object):
+ # BASIC
+ HEADER = 0xFE
+ FOOTER = 0xFA
+
+ # System status
+ ROBOT_VERSION = 0x01
+ SOFTWARE_VERSION = 0x02
+ GET_ROBOT_ID = 0x03
+ OVER_LIMIT_RETURN_ZERO = 0x04
+ SET_ROBOT_ID = 0x04
+
+ GET_ERROR_INFO = 0x07
+ CLEAR_ERROR_INFO = 0x08
+ GET_ATOM_VERSION = 0x09
+
+ CLEAR_ZERO_POS = 0x0A
+ SET_SERVO_CW = 0x0B
+ GET_SERVO_CW = 0x0C
+ CLEAR_WAIST_QUEUE = 0x0D
+ SET_LIMIT_SWITCH = 0x0E
+ GET_LIMIT_SWITCH = 0x0F
+
+ SetHTSGripperTorque = 0x35
+ GetHTSGripperTorque = 0x36
+ GetGripperProtectCurrent = 0x37
+ JOG_INCREMENT_BASE_COORD = 0x37
+ InitGripper = 0x38
+ SetGripperProtectCurrent = 0x39
+
+ # Overall status
+ POWER_ON = 0x10
+ POWER_OFF = 0x11
+ IS_POWER_ON = 0x12
+ RELEASE_ALL_SERVOS = 0x13
+ IS_CONTROLLER_CONNECTED = 0x14
+ READ_NEXT_ERROR = 0x15
+ SET_FRESH_MODE = 0x16
+ GET_FRESH_MODE = 0x17
+ SET_FREE_MODE = 0x1A
+ IS_FREE_MODE = 0x1B
+ COBOTX_GET_ANGLE = 0x1C
+ POWER_ON_ONLY = 0x1D
+ SET_CONTROL_MODE = 0x1E
+ GET_CONTROL_MODE = 0x1F
+ FOCUS_ALL_SERVOS = 0x18
+ GO_ZERO = 0x19
+ SET_BREAK = 0x19
+
+ # MDI MODE AND OPERATION
+ GET_ANGLES = 0x20
+ SEND_ANGLE = 0x21
+ SEND_ANGLES = 0x22
+ GET_COORDS = 0x23
+ SEND_COORD = 0x24
+ SEND_COORDS = 0x25
+ PAUSE = 0x26
+ IS_PAUSED = 0x27
+ RESUME = 0x28
+ STOP = 0x29
+ IS_IN_POSITION = 0x2A
+ IS_MOVING = 0x2B
+ GET_ANGLE = 0x2C
+ GET_COORD = 0x2D
+ SEND_ANGLES_AUTO = 0x2E
+ GET_SOLUTION_ANGLES = 0x2E
+ SET_SOLUTION_ANGLES = 0x2F
+
+ # JOG MODE AND OPERATION
+ JOG_ANGLE = 0x30
+ JOG_ABSOLUTE = 0x31
+ JOG_COORD = 0x32
+ JOG_INCREMENT = 0x33
+ JOG_INCREMENT_COORD = 0x34
+
+ JOG_STOP = 0x34
+ JOG_INCREMENT_COORD = 0x34
+
+ COBOTX_GET_SOLUTION_ANGLES = 0x35
+ COBOTX_SET_SOLUTION_ANGLES = 0x36
+ JOG_BASE_INCREMENT_COORD = 0x37
+
+ SET_ENCODER = 0x3A
+ GET_ENCODER = 0x3B
+ SET_ENCODERS = 0x3C
+ GET_ENCODERS = 0x3D
+ SET_ENCODERS_DRAG = 0x3E
+
+ # RUNNING STATUS AND SETTINGS
+ GET_SPEED = 0x40
+ SET_SPEED = 0x41
+ GET_FEED_OVERRIDE = 0x42
+ GET_MAX_ACC = 0x42
+ SET_MAX_ACC = 0x43
+ GET_ACCELERATION = 0x44
+ GET_DRAG_FIFO = 0x44
+ SET_DRAG_FIFO = 0x45
+ SET_ACCELERATION = 0x45
+ GET_DRAG_FIFO_LEN = 0x46
+ GET_JOINT_MIN_ANGLE = 0x4A
+ GET_JOINT_MAX_ANGLE = 0x4B
+ SET_JOINT_MIN = 0x4C
+ SET_JOINT_MAX = 0x4D
+
+ # SERVO CONTROL
+ IS_SERVO_ENABLE = 0x50
+ IS_ALL_SERVO_ENABLE = 0x51
+ SET_SERVO_DATA = 0x52
+ GET_SERVO_DATA = 0x53
+ SET_SERVO_CALIBRATION = 0x54
+ JOINT_BRAKE = 0x55
+ RELEASE_SERVO = 0x56
+ FOCUS_SERVO = 0x57
+ SET_GRIPPER_ENABLED = 0x58
+ GET_ZERO_POS = 0x59
+ IS_INIT_CALIBRATION = 0x5A
+
+
+ # ATOM IO
+ SET_PIN_MODE = 0x60
+ SET_DIGITAL_OUTPUT = 0x61
+ GET_DIGITAL_INPUT = 0x62
+ SET_PWM_MODE = 0x63
+ SET_PWM_OUTPUT = 0x64
+ GET_GRIPPER_VALUE = 0x65
+ SET_GRIPPER_STATE = 0x66
+ SET_GRIPPER_VALUE = 0x67
+ SET_GRIPPER_CALIBRATION = 0x68
+ IS_GRIPPER_MOVING = 0x69
+ SET_COLOR = 0x6A
+ SET_GRIPPER_TORQUE = 0x6F
+ IS_BTN_CLICKED = 0x6F
+ SET_COLOR_MYARM = 0x70
+ SET_ELECTRIC_GRIPPER = 0x6B
+ INIT_ELECTRIC_GRIPPER = 0x6C
+ SET_GRIPPER_MODE = 0x6D
+ GET_GRIPPER_MODE = 0x6E
+
+ GET_ACCEI_DATA = 0x73
+ SET_COLLISION_MODE = 0x74
+ GET_COLLISION_MODE = 0xFD
+ SET_COLLISION_THRESHOLD = 0x75
+ GET_COLLISION_THRESHOLD = 0x76
+ SET_TORQUE_COMP = 0x77
+ GET_TORQUE_COMP = 0x78
+ GET_VR_MODE = 0x79
+ SET_VR_MODE = 0x7A
+ GET_MODEL_DIRECTION = 0x7C
+ SET_MODEL_DIRECTION = 0x7D
+ GET_FILTER_LEN = 0x7E
+ SET_FILTER_LEN = 0x7F
+
+
+ # Basic
+ SET_BASIC_OUTPUT = 0xA0
+ GET_BASIC_INPUT = 0xA1
+ GET_BASE_INPUT = 0xA2
+ MERCURY_ROBOT_STATUS = 0xA2
+ MERCURY_ERROR_COUNTS = 0xA3
+ MERCURY_SET_POS_OVER_SHOOT = 0xA4
+ MERCURY_GET_POS_OVER_SHOOT = 0xA5
+ SET_BASE_PWM = 0xA5
+
+ # Linux GPIO, mode: GPIO.BCM
+ SET_GPIO_MODE = 0xAA
+ SET_GPIO_UP = 0xAB
+ SET_GPIO_OUTPUT = 0xAC
+ GET_GPIO_IN = 0xAD
+
+ # set WIFI
+ SET_SSID_PWD = 0xB0
+ GET_SSID_PWD = 0xB1
+ TOOL_SERIAL_RESTORE = 0xB1
+ TOOL_SERIAL_READY = 0xB2
+ TOOL_SERIAL_AVAILABLE = 0xB3
+ TOOL_SERIAL_READ_DATA = 0xB4
+ TOOL_SERIAL_WRITE_DATA = 0xB5
+ TOOL_SERIAL_FLUSH = 0xB6
+ TOOL_SERIAL_PEEK = 0xB7
+ TOOL_SERIAL_SET_BAUD = 0xB8
+ TOOL_SERIAL_SET_TIME_OUT = 0xB9
+ SET_SERVER_PORT = 0xB2
+
+ # Get the measured distance
+ GET_TOF_DISTANCE = 0xC0
+ GET_BASIC_VERSION = 0xC1
+ SET_COMMUNICATE_MODE = 0xC2
+ GET_COMMUNICATE_MODE = 0xC3
+
+ # Coordinate transformation
+ SET_TOOL_REFERENCE = 0x81
+ GET_TOOL_REFERENCE = 0x82
+ SET_WORLD_REFERENCE = 0x83
+ GET_WORLD_REFERENCE = 0x84
+ SET_REFERENCE_FRAME = 0x85
+ GET_REFERENCE_FRAME = 0x86
+ SET_MOVEMENT_TYPE = 0x87
+ GET_MOVEMENT_TYPE = 0x88
+ SET_END_TYPE = 0x89
+ GET_END_TYPE = 0x8A
+ WRITE_MOVE_C = 0x8C
+ SOLVE_INV_KINEMATICS = 0x8D
+
+ # Impact checking
+ SET_JOINT_CURRENT = 0x90
+ GET_JOINT_CURRENT = 0x91
+ SET_CURRENT_STATE = 0x92
+ GET_POS_OVER = 0x94
+ CLEAR_ENCODERS_ERROR = 0xEA
+ GET_DOWN_ENCODERS = 0x96
+
+ # planning speed
+ GET_PLAN_SPEED = 0xD0
+ GET_PLAN_ACCELERATION = 0xD1
+ SET_PLAN_SPEED = 0xD2
+ SET_PLAN_ACCELERATION = 0xD3
+ move_round = 0xD4
+ GET_ANGLES_COORDS = 0xD5
+ GET_QUICK_INFO = 0xD6
+ SET_FOUR_PIECES_ZERO = 0xD7
+
+ # Motor status read
+ GET_SERVO_SPEED = 0xE1
+ GET_SERVO_CURRENTS = 0xE2
+ GET_SERVO_VOLTAGES = 0xE3
+ GET_SERVO_STATUS = 0xE4
+ GET_SERVO_TEMPS = 0xE5
+ GET_SERVO_LASTPDI = 0xE6
+ SERVO_RESTORE = 0xE7
+ SET_VOID_COMPENSATE = 0xE7
+ # SET_ERROR_DETECT_MODE = 0xE8
+ # GET_ERROR_DETECT_MODE = 0xE9
+
+ MERCURY_GET_BASE_COORDS = 0xF0
+ MERCURY_SET_BASE_COORD = 0xF1
+ MERCURY_SET_BASE_COORDS = 0xF2
+ MERCURY_JOG_BASE_COORD = 0xF3
+ JOG_RPY = 0xF5
+
+ GET_MONITOR_MODE = 0xFB
+ SET_MONITOR_MODE = 0xFC
+
+ MERCURY_DRAG_TECH_SAVE = 0x70
+ MERCURY_DRAG_TECH_EXECUTE = 0x71
+ MERCURY_DRAG_TECH_PAUSE = 0x72
+ MERCURY_DRAG_TEACH_CLEAN = 0x73
+
+ GET_ROBOT_MODIFIED_VERSION = 1
+ GET_ROBOT_FIRMWARE_VERSION = 2
+ GET_ROBOT_AUXILIARY_FIRMWARE_VERSION = 3
+ GET_ROBOT_ATOM_MODIFIED_VERSION = 4
+ GET_ROBOT_TOOL_FIRMWARE_VERSION = 9
+ GET_ROBOT_SERIAL_NUMBER = 5
+ SET_ROBOT_ERROR_CHECK_STATE = 6
+ GET_ROBOT_ERROR_CHECK_STATE = 7
+ GET_ROBOT_ERROR_STATUS = 0x15
+ GET_ATOM_PRESS_STATUS = 0x6b
+ GET_ATOM_LED_COLOR = 0x6a
+ SET_ATOM_PIN_STATUS = 0x61
+ GET_ATOM_PIN_STATUS = 0x62
+ SET_MASTER_PIN_STATUS = 0x65
+ GET_MASTER_PIN_STATUS = 0x66
+ SET_AUXILIARY_PIN_STATUS = 0xa0
+ GET_AUXILIARY_PIN_STATUS = 0xa1
+ SET_SERVO_MOTOR_CLOCKWISE = 0x73
+ GET_SERVO_MOTOR_CLOCKWISE = 0Xea
+ SET_SERVO_MOTOR_COUNTER_CLOCKWISE = 0x74
+ GET_SERVO_MOTOR_COUNTER_CLOCKWISE = 0xeb
+ SET_SERVO_MOTOR_CONFIG = 0x52
+ GET_SERVO_MOTOR_CONFIG = 0x53
+ CLEAR_RECV_QUEUE = 0x19
+ GET_RECV_QUEUE_LENGTH = 0x08
+ GET_BASE_COORDS = 0xF0
+ BASE_TO_SINGLE_COORDS = 0xF1
+ COLLISION = 0xF2
+ GET_BASE_COORD = 0xF3
+ GET_ALL_BASE_COORDS = 0xF4
+ WRITE_BASE_COORD = 0xF5
+ WRITE_BASE_COORDS = 0xF6
+ JOG_INC_COORD = 0xF7
+ COLLISION_SWITCH = 0xF8
+ IS_COLLISION_ON = 0xF9
+ CLEAR_ROBOT_ERROR = 0x16
+ GET_RECV_QUEUE_SIZE = 0x17
+ SET_RECV_QUEUE_SIZE = 0x18
+has_return = [0x02, 0x03, 0x04, 0x09, 0x10, 0x11, 0x12, 0x13, 0x1c, 0x18, 0x19, 0x20, 0x23, 0x27, 0x29, 0x2A, 0x2B, 0x35, 0x4A, 0x4B,0x4C, 0x4D,
+ 0x50, 0x51, 0x56,0x57, 0x59,0x5A,0x62, 0x82, 0x84, 0x86, 0x88, 0x8A, 0xA1, 0xA2, 0xB2, 0xB3, 0xB4, 0xB5, 0xB7, 0xD6, 0xe1, 0xe2, 0xe4]
+
+
+def get_logger(name):
+ logger = logging.getLogger(name)
+ logger.setLevel(logging.DEBUG)
+
+ LOG_FORMAT = "%(asctime)s - %(levelname)s - %(message)s"
+ # DATE_FORMAT = "%m/%d/%Y %H:%M:%S %p"
+
+ formatter = logging.Formatter(LOG_FORMAT)
+ console = logging.StreamHandler()
+ console.setFormatter(formatter)
+
+ save = logging.handlers.RotatingFileHandler(
+ "server.log", maxBytes=10485760, backupCount=1)
+ save.setFormatter(formatter)
+
+ logger.addHandler(save)
+ logger.addHandler(console)
+ return logger
+
+
+class MercuryServer(object):
+
+ def __init__(self, host, port, serial_num="/dev/ttyAMA1", baud=115200):
+ """Server class
+
+ Args:
+ host: server ip address.
+ port: server port.
+ serial_num: serial number of the robot.The default is /dev/ttyAMA1.
+ baud: baud rate of the serial port.The default is 115200.
+
+ """
+ self.logger = get_logger("AS")
+ self.mc = None
+ self.serial_num = serial_num
+ self.baud = baud
+ self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ self.s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ self.lock = threading.Lock()
+ self.s.bind((host, port))
+ print("Binding succeeded!")
+ self.s.listen(1)
+ self.conn = None
+ self.stop = False
+ self.sync_mode = True
+ self.connected_event = threading.Event()
+ self.read_thread = threading.Thread(target=self.read)
+ self.read_thread.daemon = True
+ self.read_thread.start()
+ self.mc = serial.Serial(self.serial_num, self.baud, timeout=0.1)
+ self.connect()
+
+
+ def connect(self):
+ while True:
+ try:
+ print("waiting connect!------------------")
+ self.connected_event.clear()
+ time.sleep(0.1)
+ self.conn, addr = self.s.accept()
+ self.mc.reset_input_buffer()
+ while True:
+ try:
+ print("waiting data--------")
+ data = self.conn.recv(1024)
+ self.connected_event.set()
+ command = []
+ for v in data:
+ command.append(v)
+ if command == []:
+ print("close disconnect!")
+ break
+ if self.mc.isOpen() == False:
+ self.mc.open()
+ else:
+ self.logger.info("get command: {}".format(
+ [hex(v) for v in command]))
+ self.write(command)
+ except ConnectionResetError:
+ print("close disconnect!")
+ self.connected_event.clear()
+ time.sleep(0.1)
+ break
+ except Exception as e:
+ self.logger.error(traceback.format_exc())
+ break
+ except Exception as e:
+ self.logger.error(traceback.format_exc())
+ self.conn.close()
+ self.mc.close()
+
+ def _encode_int16(self, data):
+ if isinstance(data, int):
+ return [
+ ord(i) if isinstance(i, str) else i
+ for i in list(struct.pack(">h", data))
+ ]
+ else:
+ res = []
+ for v in data:
+ t = self._encode_int16(v)
+ res.extend(t)
+ return res
+
+ @classmethod
+ def crc_check(cls, command):
+ crc = 0xffff
+ for index in range(len(command)):
+ crc ^= command[index]
+ for _ in range(8):
+ if crc & 1 == 1:
+ crc >>= 1
+ crc ^= 0xA001
+ else:
+ crc >>= 1
+ if crc > 0x7FFF:
+ return list(struct.pack(">H", crc))
+ return cls._encode_int16(_, crc)
+
+ def write(self, command):
+ self.mc.write(command)
+ self.mc.flush()
+
+ def read(self):
+ while 1:
+ if not self.connected_event.is_set():
+ time.sleep(0.1)
+ continue
+ datas = b""
+ data_len = -1
+ k = 0
+ pre = 0
+ t = time.time()
+ wait_time = 0.1
+ data = None
+ while True and time.time() - t < wait_time:
+ try:
+ if self.mc.inWaiting() > 0:
+ data = self.mc.read()
+ k += 1
+ # print(datas, flush=True)
+ if data_len == 3:
+ datas += data
+ crc = self.mc.read(2)
+ if self.crc_check(datas) == [v for v in crc]:
+ datas += crc
+ if self.conn is not None:
+ self.logger.info("return datas: {}".format([hex(v) for v in datas]))
+ datas = b'###'+datas+b'###'
+ self.conn.sendall(datas)
+ break
+ if data_len == 1 and data == b"\xfa":
+ datas += data
+ # if [i for i in datas] == command:
+ # datas = b''
+ # data_len = -1
+ # k = 0
+ # pre = 0
+ # continue
+ # break
+ elif len(datas) == 2:
+ data_len = struct.unpack("b", data)[0]
+ datas += data
+ elif len(datas) > 2 and data_len > 0:
+ datas += data
+ data_len -= 1
+ elif data == b"\xfe":
+ if datas == b"":
+ datas += data
+ pre = k
+ else:
+ if k - 1 == pre:
+ datas += data
+ else:
+ datas = b"\xfe"
+ pre = k
+ else:
+ # print(".",end="",flush=True)
+ time.sleep(0.001)
+ except Exception as e:
+ self.logger.error(traceback.format_exc())
+ # break
+
+ def re_data_2(self, command):
+ r2 = re.compile(r'[[](.*?)[]]')
+ data_str = re.findall(r2, command)[0]
+ data_list = data_str.split(",")
+ data_list = [int(i) for i in data_list]
+ return data_list
+
+if __name__ == "__main__":
+ ifname = ["eth0", "wlan0"]
+ for i in ifname:
+ try:
+ s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ HOST = socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, struct.pack('256s', bytes(i, encoding="utf8")))[20:24])
+ PORT = 9000
+ if i == "wlan0":
+ print("***------ wireless connection ------***")
+ else:
+ print("***------ wired connection ------***")
+ break
+ except:
+ pass
+ print("ip: {} port: {}".format(HOST, PORT))
+ MercuryServer(HOST, PORT, "/dev/ttyAMA1", 115200)
diff --git a/demo/ultraArm_demo/3-angles_control.py b/demo/ultraArm_P340_demo/3-angles_control.py
similarity index 78%
rename from demo/ultraArm_demo/3-angles_control.py
rename to demo/ultraArm_P340_demo/3-angles_control.py
index df1eba4..724a1ac 100644
--- a/demo/ultraArm_demo/3-angles_control.py
+++ b/demo/ultraArm_P340_demo/3-angles_control.py
@@ -1,31 +1,31 @@
-from pymycobot.ultraArm import ultraArm
+from pymycobot.ultraArmP340 import ultraArmP340
import time
import serial
import serial.tools.list_ports
#以上需写在代码开头，意为导入项目包
-# ultraArm 类初始化需要两个参数：串口和波特率
+# ultraArmP340 类初始化需要两个参数：串口和波特率
# 第一个是串口字符串， 如：
# linux： "/dev/ttyUSB0"
# windows: "COM3"
# 第二个是波特率：115200
# 以下为如:
# linux:
-# ua = ultraArm("/dev/USB0", 115200)
+# ua = ultraArmP340("/dev/USB0", 115200)
# windows:
-# ua = ultraArm("COM3", 115200)
+# ua = ultraArmP340("COM3", 115200)
# 获取串口列表
plist = [
str(x).split(" - ")[0].strip() for x in serial.tools.list_ports.comports()
]
-# 初始化一个ultraArm对象
+# 初始化一个ultraArmP340对象
# 下面为 windows版本创建对象代码
-ua = ultraArm(plist[0], 115200)
-# ultraArm进行坐标运动/角度运动之前必须进行回零，否则无法获取到正确的角度/坐标
+ua = ultraArmP340(plist[0], 115200)
+# ultraArmP340进行坐标运动/角度运动之前必须进行回零，否则无法获取到正确的角度/坐标
ua.go_zero()
time.sleep(0.5)
diff --git a/demo/ultraArm_demo/4-coords_control.py b/demo/ultraArm_P340_demo/4-coords_control.py
similarity index 70%
rename from demo/ultraArm_demo/4-coords_control.py
rename to demo/ultraArm_P340_demo/4-coords_control.py
index b2894a0..c434d27 100644
--- a/demo/ultraArm_demo/4-coords_control.py
+++ b/demo/ultraArm_P340_demo/4-coords_control.py
@@ -1,31 +1,31 @@
-from pymycobot.ultraArm import ultraArm
+from pymycobot.ultraArmP340 import ultraArmP340
import time
import serial
import serial.tools.list_ports
#以上需写在代码开头，意为导入项目包
-# ultraArm 类初始化需要两个参数：串口和波特率
+# ultraArmP340 类初始化需要两个参数：串口和波特率
# 第一个是串口字符串， 如：
# linux： "/dev/ttyUSB0"
# windows: "COM3"
# 第二个是波特率：115200
# 以下为如:
# linux:
-# ua = ultraArm("/dev/USB0", 115200)
+# ua = ultraArmP340("/dev/USB0", 115200)
# windows:
-# ua = ultraArm("COM3", 115200)
+# ua = ultraArmP340("COM3", 115200)
#
# 获取串口列表
plist = [
str(x).split(" - ")[0].strip() for x in serial.tools.list_ports.comports()
]
-# 初始化一个ultraArm对象
+# 初始化一个ultraArmP340对象
# 下面为 windows版本创建对象代码
-ua = ultraArm(plist[0], 115200)
+ua = ultraArmP340(plist[0], 115200)
-# ultraArm进行坐标运动/角度运动之前必须进行回零，否则无法获取到正确的角度/坐标
+# ultraArmP340进行坐标运动/角度运动之前必须进行回零，否则无法获取到正确的角度/坐标
ua.go_zero()
time.sleep(0.5)
diff --git a/demo/ultraArm_demo/5-Palletizing_handling.py b/demo/ultraArm_P340_demo/5-Palletizing_handling.py
similarity index 94%
rename from demo/ultraArm_demo/5-Palletizing_handling.py
rename to demo/ultraArm_P340_demo/5-Palletizing_handling.py
index b3295e7..355b7f8 100644
--- a/demo/ultraArm_demo/5-Palletizing_handling.py
+++ b/demo/ultraArm_P340_demo/5-Palletizing_handling.py
@@ -1,4 +1,4 @@
-from pymycobot.ultraArm import ultraArm
+from pymycobot.ultraArmP340 import ultraArmP340
import serial
import serial.tools.list_ports
@@ -24,9 +24,9 @@
]
# 连接串口
-ua = ultraArm(plist[0],115200)
+ua = ultraArmP340(plist[0],115200)
-# ultraArm进行坐标运动/角度运动之前必须进行回零，否则无法获取到正确的角度/坐标
+# ultraArmP340进行坐标运动/角度运动之前必须进行回零，否则无法获取到正确的角度/坐标
ua.go_zero()
ua.sleep(0.5)
diff --git a/demo/ultraArm_demo/6-laser_use.py b/demo/ultraArm_P340_demo/6-laser_use.py
similarity index 62%
rename from demo/ultraArm_demo/6-laser_use.py
rename to demo/ultraArm_P340_demo/6-laser_use.py
index 104261e..4f89a2a 100644
--- a/demo/ultraArm_demo/6-laser_use.py
+++ b/demo/ultraArm_P340_demo/6-laser_use.py
@@ -1,29 +1,29 @@
-from pymycobot.ultraArm import ultraArm
+from pymycobot.ultraArmP340 import ultraArmP340
import time
import serial
import serial.tools.list_ports
#以上需写在代码开头，意为导入项目包
-# ultraArm 类初始化需要两个参数：串口和波特率
+# ultraArmP340 类初始化需要两个参数：串口和波特率
# 第一个是串口字符串， 如：
# linux： "/dev/ttyUSB0"
# windows: "COM3"
# 第二个是波特率：115200
# 以下为如:
# linux:
-# ua = ultraArm("/dev/USB0", 115200)
+# ua = ultraArmP340("/dev/USB0", 115200)
# windows:
-# ua = ultraArm("COM3", 115200)
+# ua = ultraArmP340("COM3", 115200)
#
plist = [
str(x).split(" - ")[0].strip() for x in serial.tools.list_ports.comports()
]
-# 初始化一个ultraArm对象
+# 初始化一个ultraArmP340对象
# 下面为 windows版本创建对象代码
-ua = ultraArm(plist[0], 115200)
+ua = ultraArmP340(plist[0], 115200)
ua.go_zero()
time.sleep(2)
diff --git a/demo/ultraArm_demo/8-gripper_use.py b/demo/ultraArm_P340_demo/8-gripper_use.py
similarity index 83%
rename from demo/ultraArm_demo/8-gripper_use.py
rename to demo/ultraArm_P340_demo/8-gripper_use.py
index 6e6e1e6..f49dacb 100644
--- a/demo/ultraArm_demo/8-gripper_use.py
+++ b/demo/ultraArm_P340_demo/8-gripper_use.py
@@ -2,7 +2,7 @@
import platform
import serial
import serial.tools.list_ports
-from pymycobot.ultraArm import ultraArm
+from pymycobot.ultraArmP340 import ultraArmP340
plist = [
str(x).split(" - ")[0].strip() for x in serial.tools.list_ports.comports()
@@ -10,10 +10,10 @@
# 自动选择系统并连接机械臂
if platform.system() == "Windows":
- ua = ultraArm(plist[0], 115200)
+ ua = ultraArmP340(plist[0], 115200)
ua.go_zero()
elif platform.system() == "Linux":
- ua = ultraArm('/dev/ttyUSB0', 115200)
+ ua = ultraArmP340('/dev/ttyUSB0', 115200)
ua.go_zero()
# 机械臂运动的位置
diff --git a/demo/ultraArm_demo/9-pump_use.py b/demo/ultraArm_P340_demo/9-pump_use.py
similarity index 60%
rename from demo/ultraArm_demo/9-pump_use.py
rename to demo/ultraArm_P340_demo/9-pump_use.py
index d0ee0be..bb3af19 100644
--- a/demo/ultraArm_demo/9-pump_use.py
+++ b/demo/ultraArm_P340_demo/9-pump_use.py
@@ -1,10 +1,10 @@
-from pymycobot.ultraArm import ultraArm
+from pymycobot.ultraArmP340 import ultraArmP340
import time
import serial
import serial.tools.list_ports
#输入以上代码导入工程所需要的包
-# ultraArm 类初始化需要两个参数：
+# ultraArmP340 类初始化需要两个参数：
# 第一个是串口字符串， 如：
# linux： "/dev/ttyUSB0"
# windows: "COM3"
@@ -12,20 +12,20 @@
#
# 如:
# linux:
-# ua = ultraArm("/dev/USB0", 115200)
+# ua = ultraArmP340("/dev/USB0", 115200)
# windows:
-# ua = ultraArm("COM3", 115200)
+# ua = ultraArmP340("COM3", 115200)
#
plist = [
str(x).split(" - ")[0].strip() for x in serial.tools.list_ports.comports()
]
-# 初始化一个ultraArm对象
+# 初始化一个ultraArmP340对象
# 下面为 windows版本创建对象代码
-ua = ultraArm(plist[0], 115200)
+ua = ultraArmP340(plist[0], 115200)
-# ultraArm进行坐标运动/角度运动之前必须进行回零，否则无法获取到正确的角度/坐标
+# ultraArmP340进行坐标运动/角度运动之前必须进行回零，否则无法获取到正确的角度/坐标
ua.go_zero()
time.sleep(0.5)
diff --git a/docs/MechArm_270_en.md b/docs/MechArm_270_en.md
index ffdbe4a..cc0c967 100644
--- a/docs/MechArm_270_en.md
+++ b/docs/MechArm_270_en.md
@@ -47,6 +47,11 @@ mc.send_angle(1, 40, 20)
- **function:** Clear robot error message
+#### `get_reboot_count()`
+
+- **function:** Get the number of times the machine has been restarted (calculated from the time the firmware is burned)
+- **Return value:** `int` Number of restarts
+
### 2. Overall Status
#### `power_on()`
@@ -72,7 +77,7 @@ mc.send_angle(1, 40, 20)
- Attentions：After the joint is disabled, it needs to be enabled to control within 1 second
- **Parameters**：`data`（optional）：The way to relax the joints. The default is damping mode, and if the 'data' parameter is provided, it can be specified as non damping mode (1- Undamping).
-#### `focus_servos(servo_id)`
+#### `focus_servo(servo_id)`
- **function:** Power on designated servo
@@ -114,6 +119,21 @@ mc.send_angle(1, 40, 20)
- `1`: Always execute the latest command first.
- `0`: Execute instructions sequentially in the form of a queue.
+#### `focus_all_servos()`
+
+- **Function:** All servos are powered on
+
+- **Return value:**
+- `1`: complete
+
+#### `set_vision_mode()`
+
+- **Function:** Set the vision tracking mode, limit the posture flipping of send_coords in refresh mode. (Applicable only to vision tracking function)
+
+- **Parameter:**
+ - `1`: open
+ - `0`: close
+
### 3.MDI Mode and Operation
#### `get_angles()`
@@ -182,20 +202,22 @@ mc.send_angle(1, 40, 20)
- `0` - not stop
- `-1` - error
-#### `sync_send_angles(angles, speed)`
+#### `sync_send_angles(angles, speed, timeout=15)`
- **function：** Send the angle in synchronous state and return when the target point is reached
- **Parameters:**
- `angles`: a list of degree value(`List[float]`), length 6
- `speed`: (`int`) 1 ~ 100
+ - `timeout`: default 15 s
-#### `sync_send_coords(coords, mode, speed)`
+#### `sync_send_coords(coords, speed, mode=0, timeout=15)`
- **function：** Send the coord in synchronous state and return when the target point is reached
- **Parameters:**
- `coords`: a list of coord value(`List[float]`), length 6
- `speed`: (`int`) 1 ~ 100
- `mode`: (`int`) 0 - angular（default）, 1 - linear
+ - `timeout`: default 15 s
#### `get_angles_coords()`
@@ -244,6 +266,21 @@ mc.send_angle(1, 40, 20)
- `0` not moving
- `-1` error
+#### `angles_to_coords(angles)`
+
+- **Function** : Convert angles to coordinates.
+- **Parameters:**
+ - `angles`: `list` List of floating points for all angles.
+- **Return value**: `list` List of floating points for all coordinates.
+
+#### `solve_inv_kinematics(target_coords, current_angles)`
+
+- **Function** : Convert coordinates to angles.
+- **Parameters:**
+ - `target_coords`: `list` List of floating points for all coordinates.
+ - `current_angles`: `list` List of floating points for all angles, current angles of the robot
+- **Return value**: `list` List of floating points for all angles.
+
### 4. JOG Mode and Operation
#### `jog_angle(joint_id, direction, speed)`
@@ -270,14 +307,24 @@ mc.send_angle(1, 40, 20)
- `direction`: (`int`) To control the direction of machine arm movement, `1` - forward rotation, `0` - reverse rotation
- `speed`: (`int`) 1 ~ 100
-#### `jog_increment(joint_id, increment, speed)`
+#### `jog_increment_angle(joint_id, increment, speed)`
-- **function:** Single joint angle increment control
+- **function:** Angle step, single joint angle increment control
- **Parameters**:
- `joint_id`: 1-6
- `increment`: Incremental movement based on the current position angle
- `speed`: 1 ~ 100
+#### `jog_increment_coord(id, increment, speed)`
+
+- **function:** Coord step, single coord increment control
+- **Parameters**:
+ - `id`: axis 1-6
+ - `increment`: Incremental movement based on the current position coord
+ - `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
+
#### `set_encoder(joint_id, encoder, speed)`
- **function**: Set a single joint rotation to the specified potential value
diff --git a/docs/MechArm_270_zh.md b/docs/MechArm_270_zh.md
index 338f2b0..5a9c495 100644
--- a/docs/MechArm_270_zh.md
+++ b/docs/MechArm_270_zh.md
@@ -47,6 +47,11 @@ mc.send_angle(1, 40, 20)
- **功能:** 清除机器人错误信息
+#### `get_reboot_count()`
+
+- **功能:** 获取机器重启次数（从烧录固件后开始计算）
+- **返回值：** `int` 重启次数
+
### 2. 机器人整体运行状态
#### `power_on()`
@@ -71,7 +76,7 @@ mc.send_angle(1, 40, 20)
- **功能：** 放松所有机械臂关节
- **参数**：`data`（可选）：关节放松方式，默认为阻尼模式，若提供 `data`参数可指定为非阻尼模式（1-Undamping）。
-#### `focus_servos(servo_id)`
+#### `focus_servo(servo_id)`
- **功能:** 单个舵机上电
@@ -113,6 +118,24 @@ mc.send_angle(1, 40, 20)
- `1`: 总是首先执行最新的命令。
- `0`: 以队列的形式按顺序执行指令。
+#### `focus_all_servos()`
+
+- **功能:** 所有舵机上电
+
+- **返回值:**
+- `1`: complete
+
+#### `set_vision_mode()`
+
+- **功能:** 设置视觉跟踪模式，限制刷新模式下send_coords的姿态翻转。（仅适用于视觉跟踪功能）
+
+- **参数:**
+ - `1`: 打开
+ - `0`: 关闭
+
+- **返回值:**
+ - `1`: 完成
+
### 3. MDI运行与操作
#### `get_angles()`
@@ -181,20 +204,22 @@ mc.send_angle(1, 40, 20)
- `0` - 没有停止
- `-1` - 错误
-#### `sync_send_angles(angles, speed)`
+#### `sync_send_angles(angles, speed, timeout=15)`
- **功能：** 同步状态下发送角度，到达目标点后返回
- **参数：**
- `angles`：角度值列表（`List[float]`），长度 6
- `speed`：（`int`）1 ~ 100
+ - `timeout`: 默认15秒
-#### `sync_send_coords(coords, mode, speed)`
+#### `sync_send_coords(coords, speed，mode=0, timeout=15)`
- **功能：** 同步状态下发送坐标，到达目标点后返回
- **参数：**
- `coords`：坐标值列表（`List[float]`），长度6
- `speed`：（`int`）1~100
- `mode`：（`int`）0-非线性（默认），1-直线运动
+ - `timeout`: 默认15秒
#### `get_angles_coords()`
@@ -243,6 +268,21 @@ mc.send_angle(1, 40, 20)
- `0` 未运动
- `-1` 错误
+#### `angles_to_coords(angles)`
+
+- **功能** : 将角度转为坐标。
+- **参数：**
+ - `angles`：`list` 所有角度的浮点列表。
+- **返回值**: `list` 所有坐标的浮点列表。
+
+#### `solve_inv_kinematics(target_coords, current_angles)`
+
+- **功能** : 将坐标转为角度。
+- **参数：**
+ - `target_coords`: `list` 所有坐标的浮点列表。
+ - `current_angles`: `list` 所有角度的浮点列表，机械臂当前角度
+- **返回值**: `list` 所有角度的浮点列表。
+
### 4. JOG运行与操作
#### `jog_angle(joint_id, direction, speed)`
@@ -269,22 +309,32 @@ mc.send_angle(1, 40, 20)
- `direction`: (`int`) 控制机臂运动方向，`1` - 正转，`0` - 反转
- `speed`: (`int`) 1 ~ 100
-#### `jog_increment(joint_id, increment, speed)`
+#### `jog_increment_angle(joint_id, increment, speed)`
-- **功能**：单关节角度增量控制
+- **功能**：角度步进，单关节角度增量控制
- **参数**：
- `joint_id`：1-6
- `increment`：基于当前位置角度的增量移动
- `speed`：1~100
+#### `jog_increment_coord(id, increment, speed)`
+
+- **功能**：坐标步进，单坐标增量控制
+- **参数**：
+ - `id`：坐标 id 1-6
+ - `increment`：基于当前位置坐标的增量移动
+ - `speed`：1~100
+- **返回值**：
+- `1`：完成
+
#### `set_encoder(joint_id,coder,speed)`
- **功能**：设置单关节旋转为指定的潜在值
- **参数**
- - `joint_id`：(`int`) 1-6
- - `encoder`：0~4096
- - `speed`：1~100
+ - `joint_id`：(`int`) 1-6
+ - `encoder`：0~4096
+ - `speed`：1~100
#### `get_encoder(joint_id)`
diff --git a/docs/MyCobot_280_RDKX5_en.md b/docs/MyCobot_280_RDKX5_en.md
new file mode 100644
index 0000000..94d4e77
--- /dev/null
+++ b/docs/MyCobot_280_RDKX5_en.md
@@ -0,0 +1,492 @@
+# MyCobot 280 X5 PI
+
+[toc]
+
+## Python API usage instructions
+
+API (Application Programming Interface), also known as Application Programming Interface functions, are predefined functions. When using the following function interfaces, please import our API library at the beginning by entering the following code, otherwise it will not run successfully:
+
+```python
+# Example
+from pymycobot import MyCobot280RDKX5
+
+mc = MyCobot280RDKX5('/dev/ttyS1')
+
+# Gets the current angle of all joints
+angles = mc.get_angles()
+print(angles)
+
+# Set 1 joint to move to 40 and speed to 20
+mc.send_angle(1, 40, 20)
+```
+
+### 1. System Status
+
+#### `get_modify_version()`
+- **function:** get modify version
+#### `get_system_version()`
+- **function:** get system version
+
+### 2. Overall Status
+
+#### `clear_queue()`
+- **function:** Clear the command queue
+#### `async_or_sync()`
+- **function:** Set the command execution mode
+- **Return value:**
+ - **`0`: Asynchronous mode**
+ - **`1`: Synchronous mode**
+#### `get_error_information()`
+- **function:** Obtaining robot error information
+- **Return value:**
+ - **`0`: No error message.**
+ - **`1 ~ 6`: The corresponding joint exceeds the limit position.**
+ - **`16 ~ 19`: Collision protection.**
+ - **`32`: Kinematics inverse solution has no solution.**
+ - **`33 ~ 34`: Linear motion has no adjacent solution.**
+#### `clear_error_information()`
+- **function:** Clear robot error message
+#### `power_on()`
+- **function:** Open communication with Atom.
+#### `power_off()`
+- **function:** Close communication with Atom.
+#### `is_power_on()`
+- **function:** Adjust robot arm status
+- **Return value:**
+ - **`1` - power on**
+ - **`0` - power off**
+ - **`-1` - error data**
+#### `release_all_servos(data)`
+- **function:** Relax all joints
+- **Parameters:**
+ - **data: `1` - Undamping (The default is damping)**
+#### `read_next_error()`
+- **function:** Robot Error Detection
+- **Return value:**
+ - **list len 6.**
+ - **`0` : No abnormality**
+ - **`1` : Communication disconnected**
+ - **`2` : Unstable communication**
+ - **`3` : Servo abnormality**
+#### `set_fresh_mode(mode)`
+- **function:** Set command refresh mode
+- **Parameters:**
+ - **mode: int.**
+ - **`1` - Always execute the latest command first.**
+ - **`0` - Execute instructions sequentially in the form of a queue.**
+#### `get_fresh_mode()`
+- **function:** Query sports mode
+#### `set_vision_mode(flag)`
+- **function:** Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode.
+ - **(Only for visual tracking function)**
+- **Parameters:**
+ - **flag: 0/1; `0` - close; `1` - open**
+
+### 3. Motion Control Interface
+
+#### `get_angles()`
+- **function:** Get the angle of all joints.
+- **Return value:**
+ - **list: A float list of all angle.**
+#### `send_angle(id, degree, speed)`
+- **function:** Send one angle of joint to robot arm.
+- **Parameters:**
+ - **id : Joint id(genre.Angle) int 1-6.**
+ - **angle : angle value(float).**
+ - **speed : (int) 1 ~ 100**
+#### `send_angles(angles, speed)`
+- **function:** Send the angles of all joints to robot arm.
+- **Parameters:**
+ - **angles: a list of angle values(List[float]). len 6.**
+ - **speed : (int) 1 ~ 100**
+#### `get_coords()`
+- **function:** Get the coords from robot arm, coordinate system based on base.
+- **Return value:**
+ - **list : A float list of coord .[x, y, z, rx, ry, rz]**
+#### `send_coord(id, coord, speed)`
+- **function:** Send one coord to robot arm.
+- **Parameters:**
+ - **id(int) : coord id(genre.Coord) int 1-6.**
+ - **coord(float) : coord value, mm**
+ - **speed(int) : 1 ~ 100**
+#### `send_coords(coords, speed, mode)`
+- **function:** Send all coords to robot arm.
+- **Parameters:**
+ - **coords: a list of coords value(List[float]).[x(mm), y, z, rx(angle), ry, rz]**
+ - **speed : (int) 1 ~ 100**
+ - **mode : (int) 0 - angluar, 1 - linear**
+#### `pause()`
+- **function:** Pause movement
+#### `is_paused()`
+- **function:** Judge whether the manipulator pauses or not.
+- **Return value:**
+ - **`1` - paused**
+ - **`0` - not paused**
+ - **`-1` - error**
+#### `resume()`
+- **function:** Recovery movement
+#### `stop()`
+- **function:** Stop moving
+#### `is_in_position(data, id = 0)`
+- **function:** Judge whether in the position.
+- **Parameters:**
+ - **data: A data list, angles or coords.len 6.**
+ - **id: 1 - coords, 0 - angles**
+- **Return value:**
+ - **`1` - True**
+ - **`0` - False**
+ - **`-1` - Error**
+#### `is_moving()`
+- **function:** Detect if the robot is moving
+- **Return value:**
+ - **`0` - not moving**
+ - **`1` - is moving**
+ - **`-1` - error data**
+#### `write_angles_time_control(angles, step_time)`
+- **function:** Write the angle of a joint in time control mode
+- **Parameters:**
+ - **angles: Angle value**
+ - **step_time: Time unit: 30ms, range(1 ~ 255)**
+
+### 4. JOG Mode And Operation
+
+#### `jog_angle(joint_id, direction, speed)`
+- **function:** Jog control angle.
+- **Parameters:**
+ - **joint_id: int 1-6.**
+ - **direction: `0` - decrease, `1` - increase**
+ - **speed: int (0 - 100)**
+#### `jog_rpy(end_direction, direction, speed)`
+- **function:** Rotate the end around a fixed axis in the base coordinate system
+- **Parameters:**
+ - **end_direction (int): Roll, Pitch, Yaw (1-3)**
+ - **direction (int): `1` - forward rotation, `0` - reverse rotation**
+ - **speed (int): 1 ~ 100**
+#### `jog_coord(coord_id, direction, speed)`
+- **function:** Jog control coord.
+- **Parameters:**
+ - **coord_id: int 1-6**
+ - **direction: `0` - decrease, `1` - increase**
+ - **speed: int (1 - 100)**
+#### `jog_increment_angle(joint_id, increment, speed)`
+- **function:** angle step mode
+- **Parameters:**
+ - **joint_id: int 1-6.**
+ - **increment: Angle increment value**
+ - **speed: int (0 - 100)**
+#### `jog_increment_coord(axis, increment, speed)`
+- **function:** coord step mode
+- **Parameters:**
+ - **axis: axis id 1 - 6.**
+ - **increment: Coord increment value**
+ - **speed: int (1 - 100)**
+#### `set_HTS_gripper_torque(torque)`
+- **function:** Set new adaptive gripper torque
+- **Parameters:**
+ - **torque (int): 150 ~ 980**
+- **Return value:**
+ - **0: Set failed**
+ - **1: Set successful**
+#### `get_HTS_gripper_torque()`
+- **function:** Get gripper torque
+- **Return value:**
+ - **int: 150 ~ 980**
+#### `get_gripper_protect_current()`
+- **function:** Get the gripper protection current
+- **Return value:**
+ - **int: 1 ~ 500**
+#### `init_gripper()`
+- **function:** Initialize gripper
+- **Return value:**
+ - **int: 0 or 1 (1 - success)**
+#### `set_gripper_protect_current(current)`
+- **function:** Set the gripper protection current
+- **Parameters:**
+ - **current (int): 1 ~ 500**
+#### `set_encoder(joint_id, encoder, speed)`
+- **function:** Set a single joint rotation to the specified potential value.
+- **Parameters:**
+ - **joint_id: int 1 - 6**
+ - **for gripper: Joint id 7**
+ - **encoder: The value of the set encoder.**
+ - **speed : 1 - 100**
+#### `get_encoder(joint_id)`
+- **function:** Obtain the specified joint potential value.
+- **Parameters:**
+ - **joint_id: (int) 1 - 6**
+ - **for gripper: Joint id 7**
+#### `set_encoders(encoders, sp)`
+- **function:** Set the six joints of the manipulator to execute synchronously to the specified position.
+- **Parameters:**
+ - **encoders: A encoder list. len 6.**
+ - **sp: speed 1 ~ 100**
+#### `get_encoders()`
+- **function:** Get the six joints of the manipulator
+- **Return value:**
+ - **The list of encoders**
+#### `set_encoders_drag(encoders, speeds)`
+- **function:** Send all encoders and speeds
+- **Parameters:**
+ - **encoders: encoders list.**
+ - **speeds: Obtained by the get_servo_speeds() method**
+#### `get_joint_min_angle(joint_id)`
+- **function:** Gets the minimum movement angle of the specified joint
+- **Parameters:**
+ - **joint_id: 1 - 6**
+- **Return value:**
+ - **angle value(float)**
+#### `get_joint_max_angle(joint_id)`
+- **function:** Gets the maximum movement angle of the specified joint
+- **Parameters:**
+ - **joint_id: 1 - 6**
+- **Return value:**
+ - **angle value(float)**
+#### `set_joint_min(id, angle)`
+- **function:** Set the joint minimum angle
+- **Parameters:**
+ - **id: int.**
+ - **Joint id 1 - 6**
+ - **for gripper: Joint id 7**
+ - **angle: 0 ~ 180**
+#### `set_joint_max(id, angle)`
+- **function:** Set the joint maximum angle
+- **Parameters:**
+ - **id: int.**
+ - **Joint id 1 - 6**
+ - **for gripper: Joint id 7**
+ - **angle: 0 ~ 180**
+
+### 5. Servo Control
+
+#### `is_servo_enable(servo_id)`
+- **function:** To detect the connection state of a single joint
+- **Parameters:**
+ - **servo_id: 1 - 6**
+- **Return value:**
+ - **`0` - disable**
+ - **`1` - enable**
+ - **`-1` - error**
+#### `is_all_servo_enable()`
+- **function:** Detect the connection status of all joints
+- **Return value:**
+ - **`0` - disable**
+ - **`1` - enable**
+ - **`-1` - error**
+#### `set_servo_data(servo_id, data_id, value, mode)`
+- **function:** Set the data parameters of the specified address of the steering gear
+- **Parameters:**
+ - **servo_id: Serial number of articulated steering gear. 1 - 7**
+ - **data_id: Data address.**
+ - **value: 0 - 4096**
+ - **mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.**
+#### `get_servo_data(servo_id, data_id, mode)`
+- **function:** Read the data parameter of the specified address of the steering gear.
+- **Parameters:**
+ - **servo_id: Serial number of articulated steering gear.1 - 7**
+ - **data_id: Data address.**
+ - **mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.**
+- **Return value:**
+ - **values 0 - 4096**
+#### `set_servo_calibration(servo_id)`
+- **function:** The current position of the calibration joint actuator is the angle zero point,
+ - **and the corresponding potential value is 2048.**
+- **Parameters:**
+ - **servo_id: Serial number of articulated steering gear. 1 - 6**
+#### `joint_brake(joint_id)`
+- **function:** Make it stop when the joint is in motion, and the buffer distance is positively related to the existing speed
+- **Parameters:**
+ - **joint_id: 1 - 6**
+#### `release_servo(servo_id, mode)`
+- **function:** Power off designated servo
+- **Parameters:**
+ - **servo_id: int 1 - 6**
+ - **mode: Default damping, set to 1, cancel damping**
+#### `focus_servo(servo_id)`
+- **function:** Power on designated servo
+- **Parameters:**
+ - **servo_id: int 1 - 6**
+
+### 6. Gripper Control
+
+#### `get_gripper_value(gripper_type)`
+- **function:** Get the value of gripper.
+- **Parameters:**
+ - **gripper_type (int): default 1**
+ - **`1`: Adaptive gripper**
+ - **`3`: Parallel gripper**
+ - **`4`: Flexible gripper**
+- **Return value:**
+ - **gripper value (int)**
+#### `set_gripper_state(flag, speed, _type_1, is_torque)`
+- **function:** Set gripper switch state
+- **Parameters:**
+ - **flag (int): 0 - open, 1 - close, 254 - release**
+ - **speed (int): 1 ~ 100**
+ - **_type_1 (int): default 1**
+ - **`1` : Adaptive gripper. default to adaptive gripper**
+ - **`2` : 5 finger dexterous hand**
+ - **`3` : Parallel gripper, this parameter can be omitted**
+ - **`4` : Flexible gripper**
+ - **is_torque (int): When there is no type parameter, this parameter can be omitted.**
+ - **`1`: Force control**
+ - **`0`: Non-force control**
+#### `set_gripper_value(gripper_value, speed, gripper_type, is_torque)`
+- **function:** Set gripper value
+- **Parameters:**
+ - **gripper_value (int): 0 ~ 100**
+ - **speed (int): 1 ~ 100**
+ - **gripper_type (int): default 1**
+ - **`1`: Adaptive gripper**
+ - **`3`: Parallel gripper, this parameter can be omitted**
+ - **`4`: Flexible gripper**
+ - **is_torque (int): When there is no type parameter, this parameter can be omitted.**
+ - **`1`: Force control**
+ - **`0`: Non-force control**
+#### `set_gripper_calibration()`
+- **function:** Set the current position to zero, set current position value is `2048`.
+#### `is_gripper_moving()`
+- **function:** Judge whether the gripper is moving or not
+- **Return value:**
+ - **`0` - not moving**
+ - **`1` - is moving**
+ - **`-1` - error data**
+
+### 7. End ATOM Function
+
+#### `get_tool_system_version()`
+- **function:** Read the terminal primary and minor version numbers
+#### `get_tool_modify_version()`
+- **function:** Read the terminal modified version number
+#### `is_tool_connected()`
+- **function:** Check the end connection status
+#### `set_color(r = 0, g = 0, b = 0)`
+- **function:** Set the light color on the top of the robot arm.
+- **Parameters:**
+ - **r (int): 0 ~ 255**
+ - **g (int): 0 ~ 255**
+ - **b (int): 0 ~ 255**
+#### `is_tool_button_click()`
+- **function:** Check whether the button on the end is pressed
+#### `set_digital_output(pin_no, pin_signal)`
+- **function:** Set the terminal atom io status
+- **Parameters:**
+ - **pin_no (int):**
+ - **pin_signal (int): 0 / 1**
+#### `get_digital_input(pin_no)`
+- **function:** singal value
+
+### 8. Kinematic Algorithm Interface
+
+#### `solve_inv_kinematics(target_coords, current_angles)`
+- **function:** Convert target coordinates to angles
+- **Parameters:**
+ - **target_coords: A float list of all coordinates.**
+ - **current_angles : A float list of all angle.**
+- **Return value:**
+ - **list: A float list of all angle.**
+
+### 9. Coordinate System Interface
+
+#### `set_tool_reference(coords)`
+- **function:** Set tool coordinate system
+- **Parameters:**
+ - **coords: a list of coords value(List[float])**
+ - **[x(mm), y, z, rx(angle), ry, rz]**
+#### `get_tool_reference()`
+- **function:** Get tool coordinate system
+#### `set_world_reference(coords)`
+- **function:** Set the world coordinate system
+- **Parameters:**
+ - **coords: a list of coords value(List[float])**
+ - **[x(mm), y, z, rx(angle), ry, rz]**
+#### `get_world_reference()`
+- **function:** Get the world coordinate system
+#### `set_reference_frame(rftype)`
+- **function:** Set the base coordinate system
+- **Parameters:**
+ - **rftype: 0 - base 1 - tool.**
+#### `get_reference_frame()`
+- **function:** Get the base coordinate system
+- **Return value:**
+ - **`0` - base `1` - tool.**
+#### `set_movement_type(move_type)`
+- **function:** Set movement type
+- **Parameters:**
+ - **move_type: `1` - movel, `0` - moveJ**
+#### `get_movement_type()`
+- **function:** Get movement type
+- **Return value:**
+ - **`1` - movel, `0` - moveJ**
+#### `set_end_type(end)`
+- **function:** Set end coordinate system
+- **Parameters:**
+ - **end: int**
+ - **`0` - flange, `1` - tool**
+#### `get_end_type()`
+- **function:** Get end coordinate system
+- **Return value:**
+ - **`0` - flange, `1` - tool**
+
+### 10. 9G Servo machine backgammon
+
+#### `move_round()`
+- **function:** Drive the 9g steering gear clockwise for one revolution
+#### `set_four_pieces_zero()`
+- **function:** Set the zero position of the four-piece motor
+- **Return value:**
+ - **int: `0` or `1` (1 - success)**
+
+### 11. Stdio Interface
+
+#### `get_angles_coords()`
+- **function:** Get joint angles and coordinates
+#### `get_quick_move_message()`
+- **function:** Get the quick move message
+#### `get_servo_speeds()`
+- **function:** Get joint speed
+- **Return value:**
+ - **A list unit step/s**
+
+### 12. Servo State Value Interface
+
+#### `get_servo_currents()`
+- **function:** Get all joint current
+- **Return value:**
+ - **A list unit mA**
+#### `get_servo_voltages()`
+- **function:** Get joint voltages
+- **Return value:**
+ - **A list volts < 24 V**
+#### `get_servo_status()`
+- **function:** Get joint status
+- **Return value:**
+ - **[voltage, sensor, temperature, current, angle, overload], a value of 0 means no error, a value of 1 indicates an error**
+#### `get_servo_temps()`
+- **function:** Get joint temperature
+- **Return value:**
+ - **A list unit ��**
+
+### 13. Drag Track Interface
+
+#### `drag_start_record()`
+- **function:** Start track recording
+- **Return value:**
+ - **Recording queue length**
+#### `drag_end_record()`
+- **function:** End track recording
+- **Return value:**
+ - **Recording queue length**
+#### `drag_get_record_data()`
+- **function:** Get the recorded track
+- **Return value:**
+ - **List of potential values (encoder values) and operating speeds of each joint**
+ - **eg: [J1_encoder, J1_run_speed,J2_encoder, J2_run_speed,J3_encoder, J3_run_speed,J4_encoder, J4_run_speed,J5_**
+ - **encoder, J5_run_speed,J6_encoder, J6_run_speed]**
+#### `drag_get_record_len()`
+- **function:** Get the total number of recorded points
+- **Return value:**
+ - **Recording queue length**
+#### `drag_clear_record_data()`
+- **function:** Clear recording track
+- **Return value:**
+ - **Recording queue length 0**
\ No newline at end of file
diff --git a/docs/MyCobot_280_RDKX5_zh.md b/docs/MyCobot_280_RDKX5_zh.md
new file mode 100644
index 0000000..85822b9
--- /dev/null
+++ b/docs/MyCobot_280_RDKX5_zh.md
@@ -0,0 +1,490 @@
+# MyCobot 280 X5 PI
+
+[toc]
+
+## Python API ʹ��˵��
+
+API��Application Programming Interface����Ӧ�ó����̽ӿڣ���Ԥ�ȶ���ĺ�����ʹ�����º����ӿ�ʱ�������ڿ�ͷ�������ǵ�API�⣬�������ɹ����У�
+
+```python
+# ʾ��
+from pymycobot import MyCobot280RDKX5
+
+mc = MyCobot280RDKX5('/dev/ttyS1')
+
+# ��ȡ���йؽڵ�ǰ�Ƕ�
+angles = mc.get_angles()
+print(angles)
+
+# ����1�Źؽ��ƶ���40�ȣ��ٶ�20
+mc.send_angle(1, 40, 20)
+```
+
+### 1. ϵͳ״̬
+
+#### `get_modify_version()`
+- **����:** ��ȡ�İ汾��
+#### `get_system_version()`
+- **����:** ��ȡϵͳ�汾
+
+### 2. ����״̬
+
+#### `clear_queue()`
+- **����:** ����������
+#### `async_or_sync()`
+- **����:** ��������ִ��ģʽ
+- **����ֵ:**
+ - **`0`: �첽ģʽ**
+ - **`1`: ͬ��ģʽ**
+#### `get_error_information()`
+- **����:** ��ȡ�����˴�����Ϣ
+- **����ֵ:**
+ - **`0`: ����**
+ - **`1~6`: ��Ӧ�ؽڳ���**
+ - **`16~19`: ��ײ����**
+ - **`32`: ���˶�ѧ��**
+ - **`33~34`: ֱ���˶������ڽ�**
+#### `clear_error_information()`
+- **����:** ��������˴�����Ϣ
+#### `power_on()`
+- **����:** ������Atomͨ��
+#### `power_off()`
+- **����:** �ر���Atomͨ��
+#### `is_power_on()`
+- **����:** ����е�۵�Դ״̬
+- **����ֵ:**
+ - **`1` - ���ϵ�**
+ - **`0` - �Ѷϵ�**
+ - **`-1` - ���ݴ���**
+#### `release_all_servos(data)`
+- **����:** �ͷ����йؽ�
+- **����:**
+ - **data: `1` - ȡ������(Ĭ������)**
+#### `read_next_error()`
+- **����:** �����˴�����
+- **����ֵ:**
+ - **����6���б�**
+ - **`0`: ����**
+ - **`1`: ͨ�ŶϿ�**
+ - **`2`: ͨ�Ų��ȶ�**
+ - **`3`: ����쳣**
+#### `set_fresh_mode(mode)`
+- **����:** ��������ˢ��ģʽ
+- **����:**
+ - **mode: int**
+ - **`1` - ����ִ����������**
+ - **`0` - ����˳��ִ��**
+#### `get_fresh_mode()`
+- **����:** ��ѯ�˶�ģʽ
+#### `set_vision_mode(flag)`
+- **����:** �����Ӿ�����ģʽ(����ˢ��ģʽ��send_coords��λ�˷�ת)
+ - **(�����Ӿ����ٹ���ʹ��)**
+- **����:**
+ - **flag: 0/1; `0` - �ر�; `1` - ����**
+
+### 3. �˶����ƽӿ�
+
+#### `get_angles()`
+- **����:** ��ȡ���йؽڽǶ�
+- **����ֵ:**
+ - **list: �Ƕȸ������б�**
+#### `send_angle(id, degree, speed)`
+- **����:** ���͵����ؽڽǶ�
+- **����:**
+ - **id : �ؽ�ID(1-6)**
+ - **angle : �Ƕ�ֵ(float)**
+ - **speed : (int) 1~100**
+#### `send_angles(angles, speed)`
+- **����:** �������йؽڽǶ�
+- **����:**
+ - **angles: �Ƕ�ֵ�б�(List[float])������6**
+ - **speed : (int) 1~100**
+#### `get_coords()`
+- **����:** ��ȡ������ϵ����
+- **����ֵ:**
+ - **list: ���긡�����б�[x, y, z, rx, ry, rz]**
+#### `send_coord(id, coord, speed)`
+- **����:** ���͵�������ֵ
+- **����:**
+ - **id(int) : ������ID(1-6)**
+ - **coord(float) : ����ֵ(mm)**
+ - **speed(int) : 1~100**
+#### `send_coords(coords, speed, mode)`
+- **����:** ������������ֵ
+- **����:**
+ - **coords: ����ֵ�б�(List[float])[x(mm), y, z, rx(�Ƕ�), ry, rz]**
+ - **speed : (int) 1~100**
+ - **mode : (int) 0 - �Ƕ�ģʽ, 1 - ֱ��ģʽ**
+#### `pause()`
+- **����:** ��ͣ�˶�
+#### `is_paused()`
+- **����:** �жϻ�е���Ƿ���ͣ
+- **����ֵ:**
+ - **`1` - ����ͣ**
+ - **`0` - ������**
+ - **`-1` - ����**
+#### `resume()`
+- **����:** �ָ��˶�
+#### `stop()`
+- **����:** ֹͣ�˶�
+#### `is_in_position(data, id = 0)`
+- **����:** �ж��Ƿ�Ŀ��λ��
+- **����:**
+ - **data: λ�������б�(�ǶȻ�����)������6**
+ - **id: 1 - ����ģʽ, 0 - �Ƕ�ģʽ**
+- **����ֵ:**
+ - **`1` - ��**
+ - **`0` - ��**
+ - **`-1` - ����**
+#### `is_moving()`
+- **����:** ����е���Ƿ��˶���
+- **����ֵ:**
+ - **`0` - ��ֹ**
+ - **`1` - �˶���**
+ - **`-1` - ���ݴ���**
+#### `write_angles_time_control(angles, step_time)`
+- **����:** ʱ�����ģʽд��ؽڽǶ�
+- **����:**
+ - **angles: �Ƕ�ֵ**
+ - **step_time: ʱ�䵥λ30ms����Χ1~255**
+
+### 4. JOGģʽ�����
+
+#### `jog_angle(joint_id, direction, speed)`
+- **����:** �ؽ�ģʽ�㶯����
+- **����:**
+ - **joint_id: ����1-6**
+ - **direction: `0` - ������, `1` - ������**
+ - **speed: ����(0-100)**
+#### `jog_rpy(end_direction, direction, speed)`
+- **����:** �ƻ�����ϵ�̶�����תĩ��
+- **����:**
+ - **end_direction (int): �����������ƫ��(1-3)**
+ - **direction (int): `1` - ������ת, `0` - ������ת**
+ - **speed (int): 1~100**
+#### `jog_coord(coord_id, direction, speed)`
+- **����:** ����ģʽ�㶯����
+- **����:**
+ - **coord_id: ����1-6**
+ - **direction: `0` - ������, `1` - ������**
+ - **speed: ����(1-100)**
+#### `jog_increment_angle(joint_id, increment, speed)`
+- **����:** �ǶȲ���ģʽ
+- **����:**
+ - **joint_id: ����1-6**
+ - **increment: �Ƕ�����ֵ**
+ - **speed: ����(0-100)**
+#### `jog_increment_coord(axis, increment, speed)`
+- **����:** ���경��ģʽ
+- **����:**
+ - **axis: ��ID 1-6**
+ - **increment: ��������ֵ**
+ - **speed: ����(1-100)**
+#### `set_HTS_gripper_torque(torque)`
+- **����:** ��������Ӧ��צŤ��
+- **����:**
+ - **torque (int): 150~980**
+- **����ֵ:**
+ - **0: ����ʧ��**
+ - **1: ���óɹ�**
+#### `get_HTS_gripper_torque()`
+- **����:** ��ȡ��צŤ��
+- **����ֵ:**
+ - **int: 150~980**
+#### `get_gripper_protect_current()`
+- **����:** ��ȡ��צ��������
+- **����ֵ:**
+ - **int: 1~500**
+#### `init_gripper()`
+- **����:** ��ʼ����צ
+- **����ֵ:**
+ - **int: 0��1(1-�ɹ�)**
+#### `set_gripper_protect_current(current)`
+- **����:** ���ü�צ��������
+- **����:**
+ - **current (int): 1~500**
+#### `set_encoder(joint_id, encoder, speed)`
+- **����:** ���õ����ؽڵ�λֵ
+- **����:**
+ - **joint_id: ����1-6**
+ - **��צ��Ӧ�ؽ�IDΪ7**
+ - **encoder: Ŀ���λֵ**
+ - **speed : 1-100**
+#### `get_encoder(joint_id)`
+- **����:** ��ȡָ���ؽڵ�λֵ
+- **����:**
+ - **joint_id: (int) 1-6**
+ - **��צ��Ӧ�ؽ�IDΪ7**
+#### `set_encoders(encoders, sp)`
+- **����:** ͬ���������йؽڵ�λֵ
+- **����:**
+ - **encoders: ��λֵ�б�������6**
+ - **sp: �ٶ�1~100**
+#### `get_encoders()`
+- **����:** ��ȡ���йؽڱ�����ֵ
+- **����ֵ:**
+ - **������ֵ�б�**
+#### `set_encoders_drag(encoders, speeds)`
+- **����:** �������б�����ֵ���ٶ�
+- **����:**
+ - **encoders: �������б�**
+ - **speeds: ͨ��get_servo_speeds()������ȡ���ٶ�ֵ**
+#### `get_joint_min_angle(joint_id)`
+- **����:** ��ȡָ���ؽ���С�Ƕ�
+- **����:**
+ - **joint_id: 1-6**
+- **����ֵ:**
+ - **�Ƕ�ֵ(float)**
+#### `get_joint_max_angle(joint_id)`
+- **����:** ��ȡָ���ؽ����Ƕ�
+- **����:**
+ - **joint_id: 1-6**
+- **����ֵ:**
+ - **�Ƕ�ֵ(float)**
+#### `set_joint_min(id, angle)`
+- **����:** ���ùؽ���С�Ƕ�
+- **����:**
+ - **id: int**
+ - **�ؽ�ID 1-6**
+ - **��צ��Ӧ�ؽ�IDΪ7**
+ - **angle: 0~180**
+#### `set_joint_max(id, angle)`
+- **����:** ���ùؽ����Ƕ�
+- **����:**
+ - **id: int**
+ - **�ؽ�ID 1-6**
+ - **��צ��Ӧ�ؽ�IDΪ7**
+ - **angle: 0~180**
+
+### 5. �������
+
+#### `is_servo_enable(servo_id)`
+- **����:** ��ⵥ���ؽ�����״̬
+- **����:**
+ - **servo_id: 1-6**
+- **����ֵ:**
+ - **`0` - δʹ��**
+ - **`1` - ��ʹ��**
+ - **`-1` - ����**
+#### `is_all_servo_enable()`
+- **����:** ������йؽ�����״̬
+- **����ֵ:**
+ - **`0` - δʹ��**
+ - **`1` - ��ʹ��**
+ - **`-1` - ����**
+#### `set_servo_data(servo_id, data_id, value, mode)`
+- **����:** ���ö��ָ����ַ����
+- **����:**
+ - **servo_id: �ؽڶ�����1-7**
+ - **data_id: ���ݵ�ַ**
+ - **value: 0-4096**
+ - **mode: 0 - ���ֽ�ֵ(Ĭ��), 1 - ˫�ֽ�ֵ**
+#### `get_servo_data(servo_id, data_id, mode)`
+- **����:** ��ȡ���ָ����ַ����
+- **����:**
+ - **servo_id: �ؽڶ�����1-7**
+ - **data_id: ���ݵ�ַ**
+ - **mode: 0 - ���ֽ�ֵ(Ĭ��), 1 - ˫�ֽ�ֵ**
+- **����ֵ:**
+ - **0-4096��Χ��ֵ**
+#### `set_servo_calibration(servo_id)`
+- **����:** У�ؽڵ�ǰλ��Ϊ���(��Ӧ��λֵ2048)
+- **����:**
+ - **servo_id: �ؽڶ�����1-6**
+#### `joint_brake(joint_id)`
+- **����:** �ؽ��˶��м�ͣ(��������뵱ǰ�ٶ������)
+- **����:**
+ - **joint_id: 1-6**
+#### `release_servo(servo_id, mode)`
+- **����:** �µ�ָ�����
+- **����:**
+ - **servo_id: ����1-6**
+ - **mode: Ĭ�����ᣬ��Ϊ1ȡ������**
+#### `focus_servo(servo_id)`
+- **����:** �ϵ�ָ�����
+- **����:**
+ - **servo_id: ����1-6**
+
+### 6. ��צ����
+
+#### `get_gripper_value(gripper_type)`
+- **����:** ��ȡ��צ״ֵ̬
+- **����:**
+ - **gripper_type (int): Ĭ��1**
+ - **`1`: ����Ӧ��צ**
+ - **`3`: ƽ�м�צ**
+ - **`4`: ���Լ�צ**
+- **����ֵ:**
+ - **��צ״ֵ̬(int)**
+#### `set_gripper_state(flag, speed, _type_1, is_torque)`
+- **����:** ���ü�צ����״̬
+- **����:**
+ - **flag (int): 0 - ��, 1 - �ر�, 254 - �ͷ�**
+ - **speed (int): 1~100**
+ - **_type_1 (int): Ĭ��1**
+ - **`1` : ����Ӧ��צ**
+ - **`2` : ��ָ������**
+ - **`3` : ƽ�м�צ(��ʡ��)**
+ - **`4` : ���Լ�צ**
+ - **is_torque (int): �����Ͳ���ʱ��ʡ��**
+ - **`1`: ����**
+ - **`0`: ������**
+#### `set_gripper_value(gripper_value, speed, gripper_type, is_torque)`
+- **����:** ���ü�צ���϶�
+- **����:**
+ - **gripper_value (int): 0~100**
+ - **speed (int): 1~100**
+ - **gripper_type (int): Ĭ��1**
+ - **`1`: ����Ӧ��צ**
+ - **`3`: ƽ�м�צ(��ʡ��)**
+ - **`4`: ���Լ�צ**
+ - **is_torque (int): �����Ͳ���ʱ��ʡ��**
+ - **`1`: ����**
+ - **`0`: ������**
+#### `set_gripper_calibration()`
+- **����:** ���õ�ǰλ��Ϊ���(��Ӧ��λֵ2048)
+#### `is_gripper_moving()`
+- **����:** �жϼ�צ�Ƿ��˶���
+- **����ֵ:**
+ - **`0` - ��ֹ**
+ - **`1` - �˶���**
+ - **`-1` - ���ݴ���**
+
+### 7. ĩ��ATOM����
+
+#### `get_tool_system_version()`
+- **����:** ��ȡĩ�����ΰ汾��
+#### `get_tool_modify_version()`
+- **����:** ��ȡĩ���İ汾��
+#### `is_tool_connected()`
+- **����:** ���ĩ������״̬
+#### `set_color(r = 0, g = 0, b = 0)`
+- **����:** ���û�е�۶����ƹ���ɫ
+- **����:**
+ - **r (int): 0~255**
+ - **g (int): 0~255**
+ - **b (int): 0~255**
+#### `is_tool_button_click()`
+- **����:** ���ĩ�˰�ť�Ƿ���
+#### `set_digital_output(pin_no, pin_signal)`
+- **����:** ����ĩ��Atom IO״̬
+- **����:**
+ - **pin_no (int): ���ź�**
+ - **pin_signal (int): 0/1**
+#### `get_digital_input(pin_no)`
+- **����:** ��ȡ���������ź�ֵ
+
+### 8. �˶�ѧ�㷨�ӿ�
+
+#### `solve_inv_kinematics(target_coords, current_angles)`
+- **����:** ��Ŀ������ת��Ϊ�ؽڽǶ�
+- **����:**
+ - **target_coords: Ŀ�����긡�����б�**
+ - **current_angles : ��ǰ�Ƕȸ������б�**
+- **����ֵ:**
+ - **list: �ؽڽǶȸ������б�**
+
+### 9. ����ϵ�ӿ�
+
+#### `set_tool_reference(coords)`
+- **����:** ���ù�������ϵ
+- **����:**
+ - **coords: ����ֵ�б�(List[float])**
+ - **[x(mm), y, z, rx(�Ƕ�), ry, rz]**
+#### `get_tool_reference()`
+- **����:** ��ȡ��������ϵ
+#### `set_world_reference(coords)`
+- **����:** ������������ϵ
+- **����:**
+ - **coords: ����ֵ�б�(List[float])**
+ - **[x(mm), y, z, rx(�Ƕ�), ry, rz]**
+#### `get_world_reference()`
+- **����:** ��ȡ��������ϵ
+#### `set_reference_frame(rftype)`
+- **����:** ���û�����ϵ
+- **����:**
+ - **rftype: 0 - ������ϵ, 1 - ��������ϵ**
+#### `get_reference_frame()`
+- **����:** ��ȡ������ϵ
+- **����ֵ:**
+ - **`0` - ������ϵ, `1` - ��������ϵ**
+#### `set_movement_type(move_type)`
+- **����:** �����˶�����
+- **����:**
+ - **move_type: `1` - ֱ���˶�, `0` - �ؽ��˶�**
+#### `get_movement_type()`
+- **����:** ��ȡ�˶�����
+- **����ֵ:**
+ - **`1` - ֱ���˶�, `0` - �ؽ��˶�**
+#### `set_end_type(end)`
+- **����:** ����ĩ������ϵ
+- **����:**
+ - **end: int**
+ - **`0` - ��������ϵ, `1` - ��������ϵ**
+#### `get_end_type()`
+- **����:** ��ȡĩ������ϵ
+- **����ֵ:**
+ - **`0` - ��������ϵ, `1` - ��������ϵ**
+
+### 10. 9G���������
+
+#### `move_round()`
+- **����:** ����9g���˳ʱ����תһ��
+#### `set_four_pieces_zero()`
+- **����:** �����ļ������λ
+- **����ֵ:**
+ - **int: `0`��`1`(1-�ɹ�)**
+
+### 11. ���ӿ�
+
+#### `get_angles_coords()`
+- **����:** ��ȡ�ؽڽǶȺ�����
+#### `get_quick_move_message()`
+- **����:** ��ȡ�����ƶ���Ϣ
+#### `get_servo_speeds()`
+- **����:** ��ȡ�ؽ��ٶ�
+- **����ֵ:**
+ - **�ٶ��б�(��λ:��/��)**
+
+### 12. ���״ֵ̬�ӿ�
+
+#### `get_servo_currents()`
+- **����:** ��ȡ���йؽڵ���
+- **����ֵ:**
+ - **�����б�(��λmA)**
+#### `get_servo_voltages()`
+- **����:** ��ȡ�ؽڵ�ѹ
+- **����ֵ:**
+ - **��ѹ�б�(��С��24V)**
+#### `get_servo_status()`
+- **����:** ��ȡ�ؽ�״̬
+- **����ֵ:**
+ - **[��ѹ, ������, �¶�, ����, �Ƕ�, ����], 0��ʾ������1��ʾ�쳣**
+#### `get_servo_temps()`
+- **����:** ��ȡ�ؽ��¶�
+- **����ֵ:**
+ - **�¶��б�(��λ��)**
+
+### 13. �϶��켣�ӿ�
+
+#### `drag_start_record()`
+- **����:** ��ʼ�켣��¼
+- **����ֵ:**
+ - **��¼���г���**
+#### `drag_end_record()`
+- **����:** �����켣��¼
+- **����ֵ:**
+ - **��¼���г���**
+#### `drag_get_record_data()`
+- **����:** ��ȡ�Ѽ�¼�켣
+- **����ֵ:**
+ - **���ؽڵ�λֵ(������ֵ)�������ٶȵ��б�**
+ - **ʾ��: [J1������ֵ,J1�����ٶ�,J2������ֵ,J2�����ٶ�,J3������ֵ,J3�����ٶ�,J4������ֵ,J4�����ٶ�,J5������ֵ,J5�����ٶ�,J6������ֵ,J6�����ٶ�]**
+#### `drag_get_record_len()`
+- **����:** ��ȡ�Ѽ�¼������
+- **����ֵ:**
+ - **��¼���г���**
+#### `drag_clear_record_data()`
+- **����:** ����Ѽ�¼�켣
+- **����ֵ:**
+ - **��¼���г��ȹ���**
diff --git a/docs/MyCobot_280_en.md b/docs/MyCobot_280_en.md
index 9596fa3..6fabe6e 100644
--- a/docs/MyCobot_280_en.md
+++ b/docs/MyCobot_280_en.md
@@ -47,6 +47,11 @@ mc.send_angle(1, 40, 20)
- **function:** Clear robot error message
+#### `get_reboot_count()`
+
+- **function:** Get the number of times the machine has been restarted (calculated from the time the firmware is burned)
+- **Return value:** `int` Number of restarts
+
### 2. Overall Status
#### `power_on()`
@@ -80,7 +85,7 @@ mc.send_angle(1, 40, 20)
- **Return value:**
- `1` - release completed.
-#### `focus_servos(servo_id)`
+#### `focus_servo(servo_id)`
- **function:** Power on designated servo
@@ -147,6 +152,24 @@ mc.send_angle(1, 40, 20)
- `1`: free mode
- `0`: on-free mode
+#### `focus_all_servos()`
+
+- **Function:** All servos are powered on
+
+- **Return value:**
+- `1`: complete
+
+#### `set_vision_mode()`
+
+- **Function:** Set the vision tracking mode, limit the posture flipping of send_coords in refresh mode. (Applicable only to vision tracking function)
+
+- **Parameter:**
+ - `1`: open
+ - `0`: close
+
+- **Return value:**
+ - `1`: complete
+
### 3.MDI Mode and Operation
#### `get_angles()`
@@ -223,24 +246,26 @@ mc.send_angle(1, 40, 20)
- `0` - not stop
- `-1` - error
-#### `sync_send_angles(angles, speed)`
+#### `sync_send_angles(angles, speed, timeout=15)`
- **function：** Send the angle in synchronous state and return when the target point is reached
- **Parameters:**
- `angles`: a list of degree value(`List[float]`), length 6
- `speed`: (`int`) 1 ~ 100
+ - `timeout`: default 15 s
- **Return value:**
- - `1`: complete
+ - `1` - complete
-#### `sync_send_coords(coords, mode, speed)`
+#### `sync_send_coords(coords, speed, mode=0, timeout=15)`
- **function：** Send the coord in synchronous state and return when the target point is reached
- **Parameters:**
- `coords`: a list of coord value(`List[float]`), length 6
- `speed`: (`int`) 1 ~ 100
- `mode`: (`int`) 0 - angular（default）, 1 - linear
+ - `timeout`: default 15 s
- **Return value:**
- - `1`: complete
+ - `1` - complete
#### `get_angles_coords()`
@@ -291,6 +316,21 @@ mc.send_angle(1, 40, 20)
- `0` not moving
- `-1` error
+#### `angles_to_coords(angles)`
+
+- **Function** : Convert angles to coordinates.
+- **Parameters:**
+ - `angles`: `list` List of floating points for all angles.
+- **Return value**: `list` List of floating points for all coordinates.
+
+#### `solve_inv_kinematics(target_coords, current_angles)`
+
+- **Function** : Convert coordinates to angles.
+- **Parameters:**
+ - `target_coords`: `list` List of floating points for all coordinates.
+ - `current_angles`: `list` List of floating points for all angles, current angles of the robot
+- **Return value**: `list` List of floating points for all angles.
+
### 4. JOG Mode and Operation
#### `jog_angle(joint_id, direction, speed)`
@@ -323,13 +363,25 @@ mc.send_angle(1, 40, 20)
- **Return value:**
- `1`: complete
-#### `jog_increment(joint_id, increment, speed)`
+#### `jog_increment_angle(joint_id, increment, speed)`
-- **function:** Single joint angle increment control
+- **function:** Angle step, single joint angle increment control
- **Parameters**:
- `joint_id`: 1-6
- `increment`: Incremental movement based on the current position angle
- `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
+
+#### `jog_increment_coord(id, increment, speed)`
+
+- **function:** Coord step, single coord increment control
+- **Parameters**:
+ - `id`: axis 1-6
+ - `increment`: Incremental movement based on the current position coord
+ - `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
#### `set_encoder(joint_id, encoder, speed)`
diff --git a/docs/MyCobot_280_zh.md b/docs/MyCobot_280_zh.md
index 5a0cd71..8fdc53e 100644
--- a/docs/MyCobot_280_zh.md
+++ b/docs/MyCobot_280_zh.md
@@ -47,6 +47,11 @@ mc.send_angle(1, 40, 20)
- **功能:** 清除机器人错误信息
+#### `get_reboot_count()`
+
+- **功能:** 获取机器重启次数（从烧录固件后开始计算）
+- **返回值：** `int` 重启次数
+
### 2. 机器人整体运行状态
#### `power_on()`
@@ -78,7 +83,7 @@ mc.send_angle(1, 40, 20)
- **返回值：**
- `1` - 释放完成。
-#### `focus_servos(servo_id)`
+#### `focus_servo(servo_id)`
- **功能:** 单个舵机上电
@@ -145,6 +150,24 @@ mc.send_angle(1, 40, 20)
- `1`: 自由移动模式
- `0`: 非自由移动模式
+#### `focus_all_servos()`
+
+- **功能:** 所有舵机上电
+
+- **返回值:**
+- `1`: complete
+
+#### `set_vision_mode()`
+
+- **功能:** 设置视觉跟踪模式，限制刷新模式下send_coords的姿态翻转。（仅适用于视觉跟踪功能）
+
+- **参数:**
+ - `1`: 打开
+ - `0`: 关闭
+
+- **返回值:**
+ - `1`: 完成
+
### 3. MDI运行与操作
#### `get_angles()`
@@ -221,24 +244,26 @@ mc.send_angle(1, 40, 20)
- `0` - 没有停止
- `-1` - 错误
-#### `sync_send_angles(angles, speed)`
+#### `sync_send_angles(angles, speed, timeout=15)`
- **功能：** 同步状态下发送角度，到达目标点后返回
- **参数：**
-- `angles`：角度值列表（`List[float]`），长度 6
-- `speed`：（`int`）1 ~ 100
+ - `angles`：角度值列表（`List[float]`），长度 6
+ - `speed`：（`int`）1 ~ 100
+ - `timeout`: 默认15秒
- **返回值：**
-- `1`：完成
+ - `1`：完成
-#### `sync_send_coords(coords, mode, speed)`
+#### `sync_send_coords(coords, speed, mode=0, timeout=15)`
- **功能：** 同步状态下发送坐标，到达目标点后返回
- **参数：**
-- `coords`：坐标值列表（`List[float]`），长度6
-- `speed`：（`int`）1~100
-- `mode`：（`int`）0-非线性（默认），1-直线运动
+ - `coords`：坐标值列表（`List[float]`），长度6
+ - `speed`：（`int`）1~100
+ - `mode`：（`int`）0-非线性（默认），1-直线运动
+ - `timeout`: 默认15秒
- **返回值：**
-- `1`：完成
+ - `1`：完成
#### `get_angles_coords()`
@@ -277,9 +302,9 @@ mc.send_angle(1, 40, 20)
- `0`: 角度
- `1`: 坐标
- **返回值**:
-- `1` - 到达
-- `0` - 未到达
-- `-1 ` - 错误
+ - `1` - 到达
+ - `0` - 未到达
+ - `-1` - 错误
#### `is_moving()`
@@ -289,6 +314,21 @@ mc.send_angle(1, 40, 20)
- `0` 未运动
- `-1` 错误
+#### `angles_to_coords(angles)`
+
+- **功能** : 将角度转为坐标。
+- **参数：**
+ - `angles`：`list` 所有角度的浮点列表。
+- **返回值**: `list` 所有坐标的浮点列表。
+
+#### `solve_inv_kinematics(target_coords, current_angles)`
+
+- **功能** : 将坐标转为角度。
+- **参数：**
+ - `target_coords`: `list` 所有坐标的浮点列表。
+ - `current_angles`: `list` 所有角度的浮点列表，机械臂当前角度
+- **返回值**: `list` 所有角度的浮点列表。
+
### 4. JOG运行与操作
#### `jog_angle(joint_id, direction, speed)`
@@ -321,22 +361,34 @@ mc.send_angle(1, 40, 20)
- **返回值：**
- `1`: 完成
-#### `jog_increment(joint_id, increment, speed)`
+#### `jog_increment_angle(joint_id, increment, speed)`
+
+- **功能**：关节步进，单关节角度增量控制
+- **参数**：
+ - `joint_id`：1-6
+ - `increment`：基于当前位置角度的增量移动
+ - `speed`：1~100
+- **返回值**：
+- `1`：完成
+
+#### `jog_increment_coord(id, increment, speed)`
-- **功能**：单关节角度增量控制
+- **功能**：坐标步进，单坐标增量控制
- **参数**：
-- `joint_id`：1-6
-- `increment`：基于当前位置角度的增量移动
-- `speed`：1~100
+ - `id`：坐标 id 1-6
+ - `increment`：基于当前位置坐标的增量移动
+ - `speed`：1~100
+- **返回值**：
+- `1`：完成
#### `set_encoder(joint_id,coder,speed)`
- **功能**：设置单关节旋转为指定的潜在值
- **参数**
- - `joint_id`：(`int`) 1-6
- - `encoder`：0~4096
- - `speed`：1~100
+ - `joint_id`：(`int`) 1-6
+ - `encoder`：0~4096
+ - `speed`：1~100
- **返回值**：
- `1`：完成
diff --git a/docs/MyCobot_320_en.md b/docs/MyCobot_320_en.md
index e7bcc56..dbb4c29 100644
--- a/docs/MyCobot_320_en.md
+++ b/docs/MyCobot_320_en.md
@@ -37,15 +37,24 @@ mc.send_angle(1, 40, 20)
- **function：** Get basic firmware version for M5 version
- **Return value：** `float` firmware version
+#### `get_reboot_count()`
+
+- **function:** Get the number of times the machine has been restarted (calculated from the time the firmware is burned)
+- **Return value:** `int` Number of restarts
+
### 2. Overall Status
#### `power_on()`
- **function:** atom open communication (default open)
+- **Return value:**
+ - `1`: completed
#### `power_off()`
- **function:** Power off of the robotic arm
+- **Return value:**
+ - `1`: completed
#### `is_power_on()`
@@ -56,18 +65,22 @@ mc.send_angle(1, 40, 20)
- `0`: power off
- `-1`: error
-#### `release_all_servos()`
+#### `release_all_servos(data=None)`
- **function:** release all robot arms
- Attentions：After the joint is disabled, it needs to be enabled to control within 1 second
- **Parameters**：`data`（optional）：The way to relax the joints. The default is damping mode, and if the 'data' parameter is provided, it can be specified as non damping mode (1- Undamping).
+- **Return value:**
+ - `1`: completed
-#### `focus_servos(servo_id)`
+#### `focus_servo(servo_id)`
- **function:** Power on designated servo
- **Parameters:**
- `servo_id:` int, 1-6
+- **Return value:**
+ - `1`: completed
#### `is_controller_connected()`
@@ -103,6 +116,45 @@ mc.send_angle(1, 40, 20)
- **Parameters:**
- `1`: Always execute the latest command first.
- `0`: Execute instructions sequentially in the form of a queue.
+- **Return value:**
+ - `1`: completed
+
+#### `get_robot_status()`
+
+- **Function:** Get the robot self-check status to see if important parameters are normal.
+
+- **Abnormal description:**
+ - 0: Communication abnormality, please check whether the line, servo firmware version is normal, whether the power is plugged in, whether the firmware is burned correctly, whether the baud rate is correct, etc.
+ - 1: The servo motor model is wrong and the motor needs to be replaced
+ - 2: The servo motor firmware version is low and needs to be upgraded using FD
+ - 3: The servo motor p value is abnormal, the default is 32, this abnormality will be automatically restored
+ - 4: The servo motor D value is abnormal, the default is 8, this abnormality will be automatically restored
+ - 5: The servo motor I value is abnormal, the default is 0, this abnormality will be automatically restored
+ - 6: The servo motor clockwise insensitive zone parameter is abnormal, the default is 3, this abnormality will be automatically restored
+ - 7: The servo motor counterclockwise insensitive zone parameter is abnormal, the default is 3, this abnormality will be automatically restored
+ - 8: The servo motor phase is abnormal, this abnormality will be automatically restored
+ - 9: The servo motor return delay is abnormal, the default is 0, this abnormality will be automatically restored
+ - 10: The servo motor minimum starting force is abnormal, the default is 0, this abnormality will be automatically restored
+ - 11: The servo motor is abnormal. When the servo is abnormal, the machine cannot be controlled. Please query the servo feedback interface get_servo_status to check the specific error.
+ - 255: Unknown error
+
+#### `focus_all_servos()`
+
+- **Function:** All servos are powered on
+
+- **Return value:**
+ - `1`: complete
+
+#### `set_vision_mode()`
+
+- **Function:** Set the vision tracking mode, limit the posture flipping of send_coords in refresh mode. (Applicable only to vision tracking function)
+
+- **Parameter:**
+ - `1`: open
+ - `0`: close
+
+- **Return value:**
+ - `1`: complete
### 3.MDI Mode and Operation
@@ -119,14 +171,16 @@ mc.send_angle(1, 40, 20)
- `degree`: degree value(`float`)
| Joint Id | range |
| ---- | ---- |
- | 1 | -170 ~ 170 |
- | 2 | -120 ~ 120 |
- | 3 | -148 ~ 148 |
- | 4 | -120 ~ 135 |
- | 5 | -169 ~ 169 |
+ | 1 | -168 ~ 168 |
+ | 2 | -135 ~ 135 |
+ | 3 | -145 ~ 145 |
+ | 4 | -148 ~ 148 |
+ | 5 | -168 ~ 168 |
| 6 | -180 ~ 180 |
- `speed`：the speed and range of the robotic arm's movement 1~100
+- **Return value:**
+ - `1`: completed
#### `send_angles(angles, speed)`
@@ -134,6 +188,8 @@ mc.send_angle(1, 40, 20)
- **Parameters:**
- `angles`: a list of degree value(`List[float]`), length 6
- `speed`: (`int`) 1 ~ 100
+- **Return value:**
+ - `1`: completed
#### `get_coords()`
@@ -155,6 +211,8 @@ mc.send_angle(1, 40, 20)
| ry | -180 ~ 180 |
| rz | -180 ~ 180 |
- `speed`: (`int`) 1-100
+- **Return value:**
+ - `1`: completed
#### `send_coords(coords, speed, mode)`
@@ -163,6 +221,8 @@ mc.send_angle(1, 40, 20)
- coords: ： a list of coords value `[x,y,z,rx,ry,rz]`,length6
- speed`(int)`: 1 ~ 100
- mode: `(int)` 0 - angluar, 1 - linear
+- **Return value:**
+ - `1`: completed
#### `pause()`
@@ -172,20 +232,26 @@ mc.send_angle(1, 40, 20)
- `0` - not stop
- `-1` - error
-#### `sync_send_angles(angles, speed)`
+#### `sync_send_angles(angles, speed, timeout=15)`
- **function：** Send the angle in synchronous state and return when the target point is reached
- **Parameters:**
- `angles`: a list of degree value(`List[float]`), length 6
- `speed`: (`int`) 1 ~ 100
+ - `timeout`: default 15 s
+- **Return value:**
+ - `1`: completed
-#### `sync_send_coords(coords, mode, speed)`
+#### `sync_send_coords(coords, speed, mode=0, timeout=15)`
- **function：** Send the coord in synchronous state and return when the target point is reached
- **Parameters:**
- `coords`: a list of coord value(`List[float]`), length 6
- `speed`: (`int`) 1 ~ 100
- `mode`: (`int`) 0 - angular（default）, 1 - linear
+ - `timeout`: default 15 s
+- **Return value:**
+ - `1`: completed
#### `get_angles_coords()`
@@ -204,6 +270,8 @@ mc.send_angle(1, 40, 20)
#### `resume()`
- **function:** resume the robot movement and complete the previous command
+- **Return value:**
+ - `1`: completed
#### `stop()`
@@ -233,6 +301,21 @@ mc.send_angle(1, 40, 20)
- `1` moving
- `0` not moving
- `-1` error
+
+#### `angles_to_coords(angles)`
+
+- **Function** : Convert angles to coordinates.
+- **Parameters:**
+ - `angles`: `list` List of floating points for all angles.
+- **Return value**: `list` List of floating points for all coordinates.
+
+#### `solve_inv_kinematics(target_coords, current_angles)`
+
+- **Function** : Convert coordinates to angles.
+- **Parameters:**
+ - `target_coords`: `list` List of floating points for all coordinates.
+ - `current_angles`: `list` List of floating points for all angles, current angles of the robot
+- **Return value**: `list` List of floating points for all angles.
### 4. JOG Mode and Operation
@@ -243,6 +326,8 @@ mc.send_angle(1, 40, 20)
- `joint_id`: Represents the joints of the robotic arm, represented by joint IDs ranging from 1 to 6
- `direction(int)`: To control the direction of movement of the robotic arm, input `0` as negative value movement and input `1` as positive value movement
- `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
#### `jog_coord(coord_id, direction, speed)`
@@ -251,6 +336,8 @@ mc.send_angle(1, 40, 20)
- `coord_id`: (`int`) Coordinate range of the robotic arm: 1~6
- `direction`: (`int`) To control the direction of machine arm movement, `0` - negative value movement, `1` - positive value movement
- `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
#### `jog_rpy(end_direction, direction, speed)`
@@ -259,14 +346,28 @@ mc.send_angle(1, 40, 20)
- `end_direction`: (`int`) Roll, Pitch, Yaw (1-3)
- `direction`: (`int`) To control the direction of machine arm movement, `1` - forward rotation, `0` - reverse rotation
- `speed`: (`int`) 1 ~ 100
+- **Return value:**
+ - `1`: completed
-#### `jog_increment(joint_id, increment, speed)`
+#### `jog_increment_angle(joint_id, increment, speed)`
-- **function:** Single joint angle increment control
+- **function:** Angle step, single joint angle increment control
- **Parameters**:
- `joint_id`: 1-6
- `increment`: Incremental movement based on the current position angle
- `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
+
+#### `jog_increment_coord(id, increment, speed)`
+
+- **function:** Coord step, single coord increment control
+- **Parameters**:
+ - `id`: axis 1-6
+ - `increment`: Incremental movement based on the current position coord
+ - `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
#### `set_encoder(joint_id, encoder, speed)`
@@ -277,6 +378,8 @@ mc.send_angle(1, 40, 20)
- `joint_id`: (`int`) 1-6
- `encoder`: 0 ~ 4096
- `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
#### `get_encoder(joint_id)`
@@ -297,6 +400,8 @@ mc.send_angle(1, 40, 20)
- `joint_id`: (`int`) 1-6
- `encoder`: 0 ~ 4096
- `speed`: 1 ~ 100
+- **Return value:**
+ - `1`: completed
#### `get_encoders()`
@@ -326,6 +431,8 @@ mc.send_angle(1, 40, 20)
- **Parameters:**
- `id` : Enter joint ID (range 1-6)
- `angle`: Refer to the limit information of the corresponding joint in the [send_angle()](#send_angleid-degree-speed) interface, which must not be less than the minimum value
+- **Return value:**
+ - `1`: completed
#### `set_joint_max(id, angle)`
@@ -333,6 +440,8 @@ mc.send_angle(1, 40, 20)
- **Parameters:**
- `id` : Enter joint ID (range 1-6)
- `angle`: Refer to the limit information of the corresponding joint in the [send_angle()](#send_angleid-degree-speed) interface, which must not be greater than the maximum value
+- **Return value:**
+ - `1`: completed
### 6. Joint motor control
@@ -358,6 +467,8 @@ mc.send_angle(1, 40, 20)
- **function:** The current position of the calibration joint actuator is the angle zero point
- **Parameters**:
- `servo_id`: 1 - 6
+- **Return value:**
+ - `1`: completed
#### `release_servo(servo_id)`
@@ -368,6 +479,8 @@ mc.send_angle(1, 40, 20)
- `1`: release successful
- `0`: release failed
- `-1`: error
+- **Return value:**
+ - `1`: completed
#### `focus_servo(servo_id)`
@@ -386,6 +499,8 @@ mc.send_angle(1, 40, 20)
- `data_id`: (`int`) Data address
- `value`: (`int`) 0 - 4096
- `mode`: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.
+- **Return value:**
+ - `1`: completed
#### `get_servo_data(servo_id, data_id, mode=None）`
@@ -401,6 +516,8 @@ mc.send_angle(1, 40, 20)
- **function:** Make it stop when the joint is in motion, and the buffer distance is positively related to the existing speed
- **Parameters**：
- `joint_id`: (`int`) joint id 1 - 6
+- **Return value:**
+ - `1`: completed
### 7. Servo state value
@@ -424,6 +541,13 @@ mc.send_angle(1, 40, 20)
- **function**：Get the movement status of all joints
- **Return value**： A list,[voltage, sensor, temperature, current, angle, overload], a value of `0` means no error, a value of `1` indicates an error
+- **Abnormal description**:
+ - 0: Servo motor undervoltage/overvoltage, check the voltage, if it is 0, you need to modify the servo parameters; if it is greater than the actual value, the heat sink may be damaged
+ - 1: Servo motor magnetic encoding abnormality
+ - 2: Servo motor overtemperature
+ - 3: Servo motor overcurrent
+ - 5: Servo motor overload
+
#### `get_servo_temps()`
- **function**：Get joint temperature
@@ -434,6 +558,8 @@ mc.send_angle(1, 40, 20)
- **function:** Set void compensation mode
- **Parameters**：
- `mode`: (`int`) 0 - close, 1 - open
+- **Return value:**
+ - `1`: completed
### 8. Robotic arm end IO control
@@ -448,6 +574,8 @@ mc.send_angle(1, 40, 20)
- `g (int)`: 0 ~ 255
- `b (int)`: 0 ~ 255
+- **Return value:**
+ - `1`: completed
#### `set_digital_output(pin_no, pin_signal)`
@@ -455,6 +583,8 @@ mc.send_angle(1, 40, 20)
- **Parameters**
- `pin_no` (int): Pin number
- `pin_signal` (int): 0 / 1
+- **Return value:**
+ - `1`: completed
#### `get_digital_input(pin_no)`
@@ -468,6 +598,8 @@ mc.send_angle(1, 40, 20)
- **Parameters**
- `pin_no` (int): Pin number
- `pin_mode` (int): 0 - input, 1 - output, 2 - input_pullup
+- **Return value:**
+ - `1`: completed
### 9. Robotic arm end gripper control
@@ -490,6 +622,8 @@ mc.send_angle(1, 40, 20)
- `3` : Parallel gripper
- `4` : Flexible gripper
+- **Return value:**
+ - `1`: completed
#### `set_gripper_value(gripper_value, speed, gripper_type=None)`
@@ -510,26 +644,36 @@ mc.send_angle(1, 40, 20)
- `3` : Parallel gripper
- `4` : Flexible gripper
+- **Return value:**
+ - `1`: completed
#### `set_gripper_calibration()`
- **function**: Set the current position of the gripper to zero
+- **Return value:**
+ - `1`: completed
-#### `init_eletric_gripper()`
+#### `init_electric_gripper()`
- **function**: Electric gripper initialization (it needs to be initialized once after inserting and removing the gripper)
+- **Return value:**
+ - `1`: completed
-#### `set_eletric_gripper(status)`
+#### `set_electric_gripper(status)`
- **function**: Set Electric Gripper Mode
- **Parameters**:
- `status`: 0 - open, 1 - close.
+- **Return value:**
+ - `1`: completed
#### `set_gripper_mode(mode)`
- **function**: Set gripper mode
- **Parameters**:
- `mode`: 0 - transparent transmission. 1 - Port Mode.
+- **Return value:**
+ - `1`: completed
#### `get_gripper_mode()`
@@ -545,6 +689,8 @@ mc.send_angle(1, 40, 20)
- **Parameters**：
- `pin_no` (`int`) Pin port number
- `pin_signal` (`int`): 0 - low. 1 - high
+- **Return value:**
+ - `1`: completed
#### `get_basic_input(pin_no)`
@@ -561,6 +707,8 @@ mc.send_angle(1, 40, 20)
- **Parameters:**
- `account` (`str`) new wifi account
- `password` (`str`) new wifi password
+- **Return value:**
+ - `1`: completed
#### `get_ssid_pwd()`
@@ -572,6 +720,8 @@ mc.send_angle(1, 40, 20)
- **function:** Change the connection port of the server
- **Parameters:**
- `port` (`int`) The new connection port of the server.
+- **Return value:**
+ - `1`: completed
### 12. TOF
@@ -587,6 +737,8 @@ mc.send_angle(1, 40, 20)
- **function:** Set tool coordinate system.
- **Parameters**：
- `coords`: (`list`) [x, y, z, rx, ry, rz].
+- **Return value:**
+ - `1`: completed
#### `get_tool_reference(coords)`
@@ -599,6 +751,8 @@ mc.send_angle(1, 40, 20)
- **function:** Set world coordinate system.
- **Parameters**：
- `coords`: (`list`) [x, y, z, rx, ry, rz].
+- **Return value:**
+ - `1`: completed
#### `get_world_reference()`
@@ -610,6 +764,8 @@ mc.send_angle(1, 40, 20)
- **function:** Set base coordinate system.
- **Parameters：**
- `rftype`: 0 - base 1 - tool.
+- **Return value:**
+ - `1`: completed
#### `get_reference_frame()`
@@ -621,6 +777,8 @@ mc.send_angle(1, 40, 20)
- **function:** Set movement type.
- **Parameters**：
- `move_type`: 1 - movel, 0 - moveJ.
+- **Return value:**
+ - `1`: completed
#### `get_movement_type()`
@@ -634,6 +792,8 @@ mc.send_angle(1, 40, 20)
- **function:** Set end coordinate system
- **Parameters:**
- `end (int)`: `0` - flange, `1` - tool
+- **Return value:**
+ - `1`: completed
#### `get_end_type()`
@@ -678,6 +838,209 @@ from pymycobot import utils
- **Return value:** Return the detected port number. If no serial port number is detected, it will return: None
+### 16. Pro force-controlled gripper
+
+#### `set_pro_gripper(gripper_id, address, value)`
+
+- **Function**: Set the parameters of the Pro force-controlled gripper. You can set a variety of parameter functions. For details, please see the table below.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+ - `address` (`int`): The command number of the gripper.
+ - `value`: The parameter value corresponding to the command number.
+
+ | Function | gripper_id | address | value|
+ | ---- | ---- |---- |----- |
+ | Set gripper ID | 14 | 3 | 1 ~ 254 |
+ | Set gripper enable status | 14 | 10 | 0 or 1, 0 - off enable; 1 - on enable |
+ | Set gripper clockwise runnable error | 14 | 21 | 0 ~ 16 |
+ | Set gripper counterclockwise runnable error | 14 | 23 | 0 ~ 16 |
+ | Set gripper minimum starting force | 14 | 25 | 0 ~ 254 |
+ | IO output settings | 14 | 29 | 0, 1, 16, 17 |
+ | Set IO opening angle | 14 | 30 | 0 ~ 100 |
+ | Set IO closing angle | 14 | 31 | 0 ~ 100 |
+ | Set servo virtual position value | 14 | 41 | 0 ~ 100 |
+ | Set clamping current | 14 | 43 | 1 ~ 254 |
+
+- **Return value**:
+ - Please refer to the following table:
+
+ | Function | return |
+ | ---- | ---- |
+ | Set gripper ID | 0 - Failure; 1 - Success |
+ | Set gripper enable status | 0 - Failure; 1 - Success |
+ | Set gripper clockwise runnable error | 0 - Failure; 1 - Success |
+ | Set gripper counterclockwise runnable error | 0 - Failure; 1 - Success |
+ | Set gripper minimum starting force | 0 - Failure; 1 - Success |
+ | IO output setting | 0 - Failure; 1 - Success |
+ | Set IO opening angle | 0 - Failure; 1 - Success |
+ | Set IO closing angle | 0 - Failed; 1 - Success |
+ | Set servo virtual position value | 0 - Failed; 1 - Success |
+ | Set holding current | 0 - Failed; 1 - Success |
+
+#### `get_pro_gripper(gripper_id, address)`
+
+- **Function**: Get the parameters of the Pro force-controlled gripper, and you can get a variety of parameter functions. For details, please see the table below.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+ - `address` (`int`): The command number of the gripper.
+
+ | Function | gripper_id | address |
+ | ---- | ---- |---- |
+ | Read firmware major version number | 14 | 1 |
+ | Read firmware minor version number | 14 | 2 |
+ | Read gripper ID | 14 | 4 |
+ | Read gripper clockwise runnable error | 14 | 22 |
+ | Read gripper counterclockwise runnable error | 14 | 24 |
+ | Read gripper minimum starting force | 14 | 26 |
+ | Read IO opening angle | 14 | 34 |
+ | Read IO closing angle | 14 | 35 |
+ | Get the amount of data in the current queue | 14 | 40 |
+ | Read servo virtual position value | 14 | 42 |
+ | Read the clamping current | 14 | 44 |
+
+- **Return value**:
+ - See the following table (if the return value is -1, it means that no data can be read):
+
+ | Function | return |
+ | ---- | ---- |
+ | Read the firmware major version number | Major version number |
+ | Read the firmware minor version number | Minor version number |
+ | Read the gripper ID | 1 ~ 254 |
+ | Read the gripper clockwise runnable error | 0 ~ 254 |
+ | Read the gripper counterclockwise runnable error | 0 ~ 254 |
+ | Read the gripper minimum starting force | 0 ~ 254 |
+ | Read the IO opening angle | 0 ~ 100 |
+ | Read the IO closing angle | 0 ~ 100 |
+ | Get the amount of data in the current queue | Return the amount of data in the current absolute control queue |
+ | Read the servo virtual position value | 0 ~ 100 |
+ | Read the clamping current | 1 ~ 254 |
+
+#### `set_pro_gripper_angle(gripper_id, gripper_angle)`
+
+- **Function**: Set the force-controlled gripper angle.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+ - `gripper_angle` (`int`): Gripper angle, value range 0 ~ 100.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `get_pro_gripper_angle(gripper_id)`
+
+- **Function**: Read the angle of the force-controlled gripper.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+- **Return value**: `int` 0 ~ 100
+
+#### `set_pro_gripper_open(gripper_id)`
+
+- **Function**: Open the force-controlled gripper.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `set_pro_gripper_close(gripper_id)`
+
+- **Function**: Close the force-controlled gripper.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `set_pro_gripper_calibration(gripper_id)`
+
+- **Function**: Set the zero position of the force-controlled gripper. (The zero position needs to be set first when using it for the first time)
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `get_pro_gripper_status(gripper_id)`
+
+- **Function**: Read the gripping status of the force-controlled gripper.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+- **Return value:**
+ - `0` - Moving.
+ - `1` - Stopped moving, no object was detected.
+ - `2` - Stopped moving, object was detected.
+ - `3` - After the object was detected, it fell.
+
+#### `set_pro_gripper_torque(gripper_id, torque_value)`
+
+- **Function**: Set the torque of the force-controlled gripper.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+ - `torque_value` (`int`): Torque value, value range 100 ~ 300.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `get_pro_gripper_torque(gripper_id)`
+
+- **Function**: Read the torque of the force-controlled gripper.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+- **Return value:** (`int`) 100 ~ 300
+
+#### `set_pro_gripper_speed(gripper_id, speed)`
+
+- **Function**: Set the force-controlled gripper speed.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+ - `speed` (int): Gripper movement speed, value range 1 ~ 100.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `get_pro_gripper_default_speed(gripper_id, speed)`
+
+- **Function**: Read the default speed of the force-controlled gripper.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+- **Return value**: Gripper default movement speed, range 1 ~ 100.
+
+#### `set_pro_gripper_abs_angle(gripper_id, gripper_angle)`
+
+- **Function**: Set the absolute angle of the force-controlled gripper.
+- **Parameter**:
+ - `gripper_id` (`int`): Gripper ID, default 14, value range 1 ~ 254.
+ - `gripper_angle` (`int`): Gripper angle, value range 0 ~ 100.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `set_pro_gripper_pause(gripper_id)`
+
+- **Function**: Pause motion.
+- **Parameter**:
+ - `gripper_id` (`int`) Gripper ID, default 14, value range 1 ~ 254.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `set_pro_gripper_resume(gripper_id)`
+
+- **Function**: Motion recovery.
+- **Parameter**:
+ - `gripper_id` (`int`) Gripper ID, default 14, value range 1 ~ 254.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
+#### `set_pro_gripper_stop(gripper_id)`
+
+- **Function**: Stop motion.
+- **Parameter**:
+ - `gripper_id` (`int`) Gripper ID, default 14, value range 1 ~ 254.
+- **Return value**:
+ - 0 - Failed
+ - 1 - Success
+
## MyCobot 320 Socket
> Note:
diff --git a/docs/MyCobot_320_zh.md b/docs/MyCobot_320_zh.md
index c169870..7ed3b9a 100644
--- a/docs/MyCobot_320_zh.md
+++ b/docs/MyCobot_320_zh.md
@@ -37,15 +37,24 @@ mc.send_angle(1, 40, 20)
- **功能:** 获取 M5 版本的basic固件版本
- **返回值：** `float` 固件版本
+#### `get_reboot_count()`
+
+- **功能:** 获取机器重启次数（从烧录固件后开始计算）
+- **返回值：** `int` 重启次数
+
### 2. 机器人整体运行状态
#### `power_on()`
- **功能:** atom 打开通信（默认打开）
+- **返回值:**
+ - `1`: 完成
#### `power_off()`
- **功能:** 机械臂掉电
+- **返回值:**
+ - `1`: 完成
#### `is_power_on()`
@@ -56,17 +65,21 @@ mc.send_angle(1, 40, 20)
- `0`: 掉电
- `-1`: 错误
-#### `release_all_servos()`
+#### `release_all_servos(data=None)`
- **功能：** 放松所有机械臂关节
- **参数**：`data`（可选）：关节放松方式，默认为阻尼模式，若提供 `data`参数可指定为非阻尼模式（1-Undamping）。
+- **返回值:**
+ - `1`: 完成
-#### `focus_servos(servo_id)`
+#### `focus_servo(servo_id)`
- **功能:** 单个舵机上电
- **参数:**
-- `servo_id:` int, 1-6
+ - `servo_id:` int, 1-6
+- **返回值:**
+ - `1`: 完成
#### `is_controller_connected()`
@@ -92,8 +105,8 @@ mc.send_angle(1, 40, 20)
- **功能:** 查询运动模式
- **返回值:**
-- `0`: 插补模式
-- `1`: 刷新模式
+ - `0`: 插补模式
+ - `1`: 刷新模式
#### `set_fresh_mode()`
@@ -102,6 +115,45 @@ mc.send_angle(1, 40, 20)
- **参数:**
- `1`: 总是首先执行最新的命令。
- `0`: 以队列的形式按顺序执行指令。
+- **返回值:**
+ - `1`: 完成
+
+#### `get_robot_status()`
+
+- **功能:** 获取机器人自检状态，看重要参数是否都正常。
+
+- **异常说明:**
+ - 0: 通信异常，请检查线路、舵机固件版本是否正常、电源是否插上、固件是否烧录正确，波特率是否正确等
+ - 1: 伺服电机型号错误，需要更换电机
+ - 2: 伺服电机固件版本较低，需要使用FD升级
+ - 3: 伺服电机p值异常，默认32，此异常会自动恢复
+ - 4: 伺服电机D值异常，默认8，此异常会自动恢复
+ - 5: 伺服电机I值异常，默认0，此异常会自动恢复
+ - 6: 伺服电机顺时针不灵敏区参数异常，默认3，此异常会自动恢复
+ - 7: 伺服电机逆时针不灵敏区参数异常，默认3，此异常会自动恢复
+ - 8: 伺服电机相位异常，此异常会自动恢复
+ - 9: 伺服电机返回延时异常，默认0，此异常会自动恢复
+ - 10: 伺服电机最小启动力异常，默认0，此异常会自动恢复
+ - 11: 伺服电机异常，当舵机异常时，无法控制机器运动，请查询舵机反馈接口get_servo_status，查看具体报错
+ - 255: 未知错误
+
+#### `focus_all_servos()`
+
+- **功能:** 所有舵机上电
+
+- **返回值:**
+- `1`: complete
+
+#### `set_vision_mode()`
+
+- **功能:** 设置视觉跟踪模式，限制刷新模式下send_coords的姿态翻转。（仅适用于视觉跟踪功能）
+
+- **参数:**
+ - `1`: 打开
+ - `0`: 关闭
+
+- **返回值:**
+ - `1`: 完成
### 3. MDI运行与操作
@@ -118,14 +170,16 @@ mc.send_angle(1, 40, 20)
- `degree`：度数值（`float`）
| 关节 Id | 范围 |
| ---- | ---- |
- | 1 | -170 ~ 170 |
- | 2 | -120 ~ 120 |
- | 3 | -148 ~ 148 |
- | 4 | -120 ~ 135 |
- | 5 | -169 ~ 169 |
+ | 1 | -168 ~ 168 |
+ | 2 | -135 ~ 135 |
+ | 3 | -145 ~ 145 |
+ | 4 | -148 ~ 148 |
+ | 5 | -168 ~ 168 |
| 6 | -180 ~ 180 |
- `speed`：机械臂运动速度及范围 1~100
+- **返回值:**
+ - `1`: 完成
#### `send_angles(angles, speed)`
@@ -133,6 +187,8 @@ mc.send_angle(1, 40, 20)
- **参数：**
- `angles`：度数列表（`List[float]`），长度 6
- `speed`：（`int`）1 ~ 100
+- **返回值:**
+ - `1`: 完成
#### `get_coords()`
@@ -154,6 +210,8 @@ mc.send_angle(1, 40, 20)
| ry | -180 ~ 180 |
| rz | -180 ~ 180 |
- `speed`：（`int`）1-100
+- **返回值:**
+ - `1`: 完成
#### `send_coords(coords, speed, mode)`
@@ -162,6 +220,8 @@ mc.send_angle(1, 40, 20)
- `coords`: 坐标列表，值`[x,y,z,rx,ry,rz]`，长度6
- `speed` (`int`)：1 ~ 100
- `mode：`(`int`) 0 - 非线性，1 - 直线运动
+- **返回值:**
+ - `1`: 完成
#### `pause()`
@@ -171,20 +231,26 @@ mc.send_angle(1, 40, 20)
- `0` - 没有停止
- `-1` - 错误
-#### `sync_send_angles(angles, speed)`
+#### `sync_send_angles(angles, speed, timeout=15)`
- **功能：** 同步状态下发送角度，到达目标点后返回
- **参数：**
- `angles`：角度值列表（`List[float]`），长度 6
- `speed`：（`int`）1 ~ 100
+ - `timeout`: 默认15秒
+- **返回值:**
+ - `1`: 完成
-#### `sync_send_coords(coords, mode, speed)`
+#### `sync_send_coords(coords, speed, mode=0, timeout=15)`
- **功能：** 同步状态下发送坐标，到达目标点后返回
- **参数：**
- `coords`：坐标值列表（`List[float]`），长度6
- `speed`：（`int`）1~100
- `mode`：（`int`）0-非线性（默认），1-直线运动
+ - `timeout`: 默认15秒
+- **返回值:**
+ - `1`: 完成
#### `get_angles_coords()`
@@ -203,6 +269,8 @@ mc.send_angle(1, 40, 20)
#### `resume()`
- **功能:** 恢复机器人运动并完成上一个命令
+- **返回值:**
+ - `1`: 完成
#### `stop()`
@@ -232,6 +300,21 @@ mc.send_angle(1, 40, 20)
- `1` 运动中
- `0` 未运动
- `-1` 错误
+
+#### `angles_to_coords(angles)`
+
+- **功能** : 将角度转为坐标。
+- **参数：**
+ - `angles`：`list` 所有角度的浮点列表。
+- **返回值**: `list` 所有坐标的浮点列表。
+
+#### `solve_inv_kinematics(target_coords, current_angles)`
+
+- **功能** : 将坐标转为角度。
+- **参数：**
+ - `target_coords`: `list` 所有坐标的浮点列表。
+ - `current_angles`: `list` 所有角度的浮点列表，机械臂当前角度
+- **返回值**: `list` 所有角度的浮点列表。
### 4. JOG运行与操作
@@ -242,6 +325,8 @@ mc.send_angle(1, 40, 20)
- `joint_id`：表示机械臂的关节，用关节ID表示，范围是1~6
- `direction(int)`：控制机械臂运动的方向，输入`0`为负值运动，输入`1`为正值运动
- `speed`：1~100
+- **返回值:**
+ - `1`: 完成
#### `jog_coord(coord_id, direction, speed)`
@@ -250,6 +335,8 @@ mc.send_angle(1, 40, 20)
- `coord_id`: (`int`) 机械臂坐标范围：1~6
- `direction`: (`int`) 控制机臂运动方向，`0` - 负值运动，`1` - 正值运动
- `speed`: 1 ~ 100
+- **返回值:**
+ - `1`: 完成
#### `jog_rpy(end_direction, direction, speed)`
@@ -258,23 +345,37 @@ mc.send_angle(1, 40, 20)
- `end_direction`: (`int`) Roll、Pitch、Yaw（1-3）
- `direction`: (`int`) 控制机臂运动方向，`1` - 正转，`0` - 反转
- `speed`: (`int`) 1 ~ 100
+- **返回值:**
+ - `1`: 完成
+
+#### `jog_increment_angle(joint_id, increment, speed)`
+
+- **功能**：角度步进，单关节角度增量控制
+- **参数**：
+ - `joint_id`：1-6
+ - `increment`：基于当前位置角度的增量移动
+ - `speed`：1~100
+- **返回值:**
+ - `1`: 完成
-#### `jog_increment(joint_id, increment, speed)`
+#### `jog_increment_coord(id, increment, speed)`
-- **功能**：单关节角度增量控制
+- **功能**：坐标步进，单坐标增量控制
- **参数**：
-- `joint_id`：1-6
-- `increment`：基于当前位置角度的增量移动
-- `speed`：1~100
+ - `id`：坐标 id 1-6
+ - `increment`：基于当前位置坐标的增量移动
+ - `speed`：1~100
+- **返回值**：
+- `1`：完成
#### `set_encoder(joint_id,coder,speed)`
- **功能**：设置单关节旋转为指定的潜在值
- **参数**
- - `joint_id`：(`int`) 1-6
- - `encoder`：0~4096
- - `speed`：1~100
+ - `joint_id`：(`int`) 1-6
+ - `encoder`：0~4096
+ - `speed`：1~100
#### `get_encoder(joint_id)`
@@ -324,6 +425,8 @@ mc.send_angle(1, 40, 20)
- **参数:**
- `id` : 输入关节ID（范围1-6）
- `angle`: 参考 [send_angle()](#send_angleid-degree-speed) 接口中对应关节的限制信息，不得小于最小值
+- **返回值:**
+ - `1`: 完成
#### `set_joint_max(id, angle)`
@@ -331,6 +434,8 @@ mc.send_angle(1, 40, 20)
- **参数：**
- `id` ：输入关节ID（范围1-6）
- `angle`：参考 [send_angle()](#send_angleid-degree-speed) 接口中对应关节的限制信息，不得大于最大值
+- **返回值:**
+ - `1`: 完成
### 6. 关节电机控制
@@ -356,6 +461,8 @@ mc.send_angle(1, 40, 20)
- **功能:** 校准指定关节，设置当前位置为角度零点，对应电位值为2048
- **参数**:
- `servo_id`: 1 - 6
+- **返回值:**
+ - `1`: 完成
#### `release_servo(servo_id)`
@@ -384,6 +491,8 @@ mc.send_angle(1, 40, 20)
- `data_id`: (`int`) 数据地址
- `value`: (`int`) 0 - 4096
- `mode`: 0 - 表示值为一个字节（默认），1 - 1 表示值为两个字节。
+- **返回值:**
+ - `1`: 完成
#### `get_servo_data(servo_id, data_id, mode=None）`
@@ -399,6 +508,8 @@ mc.send_angle(1, 40, 20)
- **功能：** 使关节在运动时停止，缓冲距离与现有速度正相关
- **参数**：
- `joint_id`: (`int`) 关节id 1 - 6
+- **返回值:**
+ - `1`: 完成
### 7. 伺服状态值
@@ -422,6 +533,13 @@ mc.send_angle(1, 40, 20)
- **功能**：获取所有关节的运动状态
- **返回值**： 列表，[电压，传感器，温度，电流，角度，过载]，值为`0`表示无错误，值为`1`表示有错误
+- **异常说明**：
+ - 0: 伺服电机欠压/过压，查看电压，如果为0，需要修改舵机参数；如果大于实际，可能散热片损坏
+ - 1: 伺服电机磁编码异常
+ - 2: 伺服电机过温
+ - 3: 伺服电机过流
+ - 5: 伺服电机过载
+
#### `get_servo_temps()`
- **功能**：获取关节温度
@@ -439,13 +557,17 @@ mc.send_angle(1, 40, 20)
- `g (int)`: 0 ~ 255
- `b (int)`: 0 ~ 255
-
+ - **返回值:**
+ - `1`: 完成
+
#### `set_digital_output(pin_no, pin_signal)`
- **功能:** 设置IO状态
- **参数**
- `pin_no` (int): 引脚号
- `pin_signal` (int): 0 / 1, 输入0表示设置为运行状态，输入1表示停止状态
+- **返回值:**
+ - `1`: 完成
#### `get_digital_input(pin_no)`
@@ -459,6 +581,8 @@ mc.send_angle(1, 40, 20)
- **参数**
- `pin_no` (int): 引脚号
- `pin_mode` (int): 0 - 运行状态, 1 - 停止状态, 2 - 上拉模式
+- **返回值:**
+ - `1`: 完成
### 9. 机械臂末端夹爪控制
@@ -481,6 +605,8 @@ mc.send_angle(1, 40, 20)
- `3` : 平行夹爪
- `4` : 柔性夹爪
+- **返回值:**
+ - `1`: 完成
#### `set_gripper_value(gripper_value, speed, gripper_type=None)`
@@ -501,26 +627,36 @@ mc.send_angle(1, 40, 20)
- `3` : 平行夹爪
- `4` : 柔性夹爪
+- **返回值:**
+ - `1`: 完成
#### `set_gripper_calibration()`
- **功能**: 将夹爪的当前位置设置为零位
+- **返回值:**
+ - `1`: 完成
-#### `init_eletric_gripper()`
+#### `init_electric_gripper()`
- **功能**：电动夹爪初始化（插入和移除夹爪后需初始化一次）
+- **返回值:**
+ - `1`: 完成
-#### `set_eletric_gripper(status)`
+#### `set_electric_gripper(status)`
- **功能**：设置电动夹爪模式
- **参数**：
- `status`：0 - 打开，1 - 关闭。
+- **返回值:**
+ - `1`: 完成
#### `set_gripper_mode(mode)`
- **功能**：设置夹爪模式
- **参数**：
- `mode`：0 - 透明传输。1 - 端口模式。
+- **返回值:**
+ - `1`: 完成
#### `get_gripper_mode()`
@@ -536,6 +672,8 @@ mc.send_angle(1, 40, 20)
- **参数**：
- `pin_no` (`int`) 设备底部标注的编号仅取数字部分
- `pin_signal` (`int`): 0 - 低电平，设置为运行状态. 1 - 高电平，停止状态。
+- **返回值:**
+ - `1`: 完成
#### `get_basic_input(pin_no)`
@@ -552,6 +690,8 @@ mc.send_angle(1, 40, 20)
- **参数:**
- `account`： (`str`) 新的 wifi 账户
- `password`: (`str`) 新的 wifi 密码
+- **返回值:**
+ - `1`: 完成
#### `get_ssid_pwd()`
@@ -563,6 +703,8 @@ mc.send_angle(1, 40, 20)
- **功能:** 更改服务器的连接端口
- **参数:**
- `port`: (`int`) 服务器的新连接端口
+- **返回值:**
+ - `1`: 完成
### 12. TOF
@@ -578,6 +720,8 @@ mc.send_angle(1, 40, 20)
- **功能:** 设置工具坐标系
- **参数**：
- `coords`: (`list`) [x, y, z, rx, ry, rz].
+- **返回值:**
+ - `1`: 完成
#### `get_tool_reference(coords)`
@@ -589,6 +733,8 @@ mc.send_angle(1, 40, 20)
- **功能:** 设置世界坐标系
- **参数**：
- `coords`: (`list`) [x, y, z, rx, ry, rz].
+- **返回值:**
+ - `1`: 完成
#### `get_world_reference()`
@@ -600,6 +746,8 @@ mc.send_angle(1, 40, 20)
- **功能:** 设置基坐标系
- **参数：**
- `rftype`: 0 - 基坐标（默认） 1 - 世界坐标.
+- **返回值:**
+ - `1`: 完成
#### `get_reference_frame()`
@@ -611,6 +759,8 @@ mc.send_angle(1, 40, 20)
- **功能:** 设置移动类型
- **参数**：
- `move_type`: 1 - movel, 0 - moveJ.
+- **返回值:**
+ - `1`: 完成
#### `get_movement_type()`
@@ -624,6 +774,8 @@ mc.send_angle(1, 40, 20)
- **功能:** 设置末端坐标系
- **参数:**
- `end (int)`: `0` - 法兰（默认）, `1` - 工具
+- **返回值:**
+ - `1`: 完成
#### `get_end_type()`
@@ -668,6 +820,209 @@ from pymycobot import utils
- **返回值:** 返回检测到的端口号，如果没有监测到串口号则返回：None
+### 16. Pro 力控夹爪
+
+#### `set_pro_gripper(gripper_id, address, value)`
+
+- **功能**：设置Pro力控夹爪参数，可以设置多种参数功能。具体请查看如下表格。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+ - `address` (`int`): 夹爪的指令序号。
+ - `value` ：指令序号对应的参数值。
+
+ | 功能 | gripper_id | address | value|
+ | ---- | ---- |---- |----- |
+ | 设置夹爪ID | 14 | 3 | 1 ~ 254 |
+ | 设置夹爪使能状态 | 14 | 10 | 0或者1, 0 - 掉使能; 1 - 上使能 |
+ | 设置夹爪顺时针可运行误差 | 14 | 21 | 0 ~ 16 |
+ | 设置夹爪逆时针可运行误差 | 14 | 23 | 0 ~ 16 |
+ | 设置夹爪最小启动力 | 14 | 25 | 0 ~ 254 |
+ | IO输出设置 | 14 | 29 | 0, 1, 16, 17 |
+ | 设置IO张开角度 | 14 | 30 | 0 ~ 100 |
+ | 设置IO闭合角度 | 14 | 31 | 0 ~ 100 |
+ | 设置舵机虚位数值 | 14 | 41 | 0 ~ 100 |
+ | 设置夹持电流 | 14 | 43 | 1 ~ 254 |
+
+- **返回值**:
+ - 请查看如下表格：
+
+ | 功能 | return |
+ | ---- | ---- |
+ | 设置夹爪ID | 0 - 失败； 1 - 成功 |
+ | 设置夹爪使能状态 | 0 - 失败； 1 - 成功 |
+ | 设置夹爪顺时针可运行误差 | 0 - 失败； 1 - 成功 |
+ | 设置夹爪逆时针可运行误差 | 0 - 失败； 1 - 成功 |
+ | 设置夹爪最小启动力 | 0 - 失败； 1 - 成功 |
+ | IO输出设置 | 0 - 失败； 1 - 成功 |
+ | 设置IO张开角度 | 0 - 失败； 1 - 成功 |
+ | 设置IO闭合角度 | 0 - 失败； 1 - 成功 |
+ | 设置舵机虚位数值 | 0 - 失败； 1 - 成功 |
+ | 设置夹持电流 | 0 - 失败； 1 - 成功 |
+
+#### `get_pro_gripper(gripper_id, address)`
+
+- **功能**：获取Pro力控夹爪参数，可以获取多种参数功能。具体请查看如下表格。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+ - `address` (`int`): 夹爪的指令序号。
+
+ | 功能 | gripper_id | address |
+ | ---- | ---- |---- |
+ | 读取固件主版本号 | 14 | 1 |
+ | 读取固件次版本号 | 14 | 2 |
+ | 读取夹爪ID | 14 | 4 |
+ | 读取夹爪顺时针可运行误差 | 14 | 22 |
+ | 读取夹爪逆时针可运行误差 | 14 | 24 |
+ | 读取夹爪最小启动力 | 14 | 26 |
+ | 读取IO张开角度 | 14 | 34 |
+ | 读取IO闭合角度 | 14 | 35 |
+ | 获取当前队列的数据量 | 14 | 40 |
+ | 读取舵机虚位数值 | 14 | 42 |
+ | 读取夹持电流 | 14 | 44 |
+
+- **返回值**：
+ - 查看如下表格（若返回值为 -1，则表示读不到数据）：
+
+ | 功能 | return |
+ | ---- | ---- |
+ | 读取固件主版本号 | 主版本号 |
+ | 读取固件次版本号 | 次版本号 |
+ | 读取夹爪ID | 1 ~ 254 |
+ | 读取夹爪顺时针可运行误差 | 0 ~ 254 |
+ | 读取夹爪逆时针可运行误差 | 0 ~ 254 |
+ | 读取夹爪最小启动力 | 0 ~ 254 |
+ | 读取IO张开角度 | 0 ~ 100 |
+ | 读取IO闭合角度 | 0 ~ 100 |
+ | 获取当前队列的数据量 | 返回当前绝对控制队列中的数据量 |
+ | 读取舵机虚位数值 | 0 ~ 100 |
+ | 读取夹持电流 | 1 ~ 254 |
+
+#### `set_pro_gripper_angle(gripper_id, gripper_angle)`
+
+- **功能**：设置力控夹爪角度。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围1 ~ 254。
+ - `gripper_angle` (`int`): 夹爪角度，取值范围 0 ~ 100。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `get_pro_gripper_angle(gripper_id)`
+
+- **功能**：读取力控夹爪角度。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值**：`int` 0 ~ 100
+
+#### `set_pro_gripper_open(gripper_id)`
+
+- **功能**：打开力控夹爪。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `set_pro_gripper_close(gripper_id)`
+
+- **功能**：关闭力控夹爪。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `set_pro_gripper_calibration(gripper_id)`
+
+- **功能**：设置力控夹爪零位。（首次使用需要先设置零位）
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `get_pro_gripper_status(gripper_id)`
+
+- **功能**：读取力控夹爪夹持状态。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值:**
+ - `0` - 正在运动。
+ - `1` - 停止运动，未检测到夹到物体。
+ - `2` - 停止运动，检测到夹到物体。
+ - `3` - 检测到夹到物体之后，物体掉落。
+
+#### `set_pro_gripper_torque(gripper_id, torque_value)`
+
+- **功能**：设置力控夹爪扭矩。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+ - `torque_value` (`int`) ：扭矩值，取值范围 100 ~ 300。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `get_pro_gripper_torque(gripper_id)`
+
+- **功能**：读取力控夹爪扭矩。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值:** (`int`) 100 ~ 300
+
+#### `set_pro_gripper_speed(gripper_id, speed)`
+
+- **功能**：设置力控夹爪速度。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+ - `speed` (int): 夹爪运动速度，取值范围 1 ~ 100。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `get_pro_gripper_default_speed(gripper_id, speed)`
+
+- **功能**：读取力控夹爪默认速度。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值**：夹爪默认运动速度，范围 1 ~ 100。
+
+#### `set_pro_gripper_abs_angle(gripper_id, gripper_angle)`
+
+- **功能**：设置力控夹爪绝对角度。
+- **参数**：
+ - `gripper_id` (`int`): 夹爪ID，默认14，取值范围 1 ~ 254。
+ - `gripper_angle` (`int`): 夹爪角度，取值范围 0 ~ 100。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `set_pro_gripper_pause(gripper_id)`
+
+- **功能**：暂停运动。
+- **参数**：
+ - `gripper_id` (`int`) 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `set_pro_gripper_resume(gripper_id)`
+
+- **功能**：运动恢复。
+- **参数**：
+ - `gripper_id` (`int`) 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
+#### `set_pro_gripper_stop(gripper_id)`
+
+- **功能**：停止运动。
+- **参数**：
+ - `gripper_id` (`int`) 夹爪ID，默认14，取值范围 1 ~ 254。
+- **返回值**：
+ - 0 - 失败
+ - 1 - 成功
+
## MyCobot 320 Socket
> 注意：
diff --git a/docs/README.md b/docs/README.md
index 8af912b..ddcafe1 100644
--- a/docs/README.md
+++ b/docs/README.md
@@ -18,6 +18,8 @@ We support Python2, Python3.5 or later.
[myArm_M&C API说明](./myArm_M&C_zh.md) | [myArm_M&C API Description](./myArm_M&C_en.md)
+[ultraArm P340 API说明](./ultraArm_P340_zh.md) | [ultraArm P340 API Description](./ultraArm_P340_en.md)
+
<details>
<summary>Catalogue:</summary>
@@ -48,7 +50,7 @@ We support Python2, Python3.5 or later.
- [get\_plan\_acceleration(id=0)](#get_plan_accelerationid0)
- [get\_plan\_speed(id=0)](#get_plan_speedid0)
- [get\_reference\_frame(id)](#get_reference_frameid)
- - [get\_robot\_id(id)](#get_robot_idid)
+ - [get\_robot\_id()](#get_robot_id)
- [get\_robot\_version(id)](#get_robot_versionid)
- [get\_servo\_currents(id)](#get_servo_currentsid)
- [get\_servo\_data(id, servo\_no, data\_id)](#get_servo_dataid-servo_no-data_id)
@@ -407,14 +409,10 @@ Get the base coordinate system
0 - base 1 - tool.
-### get_robot_id(id)
+### get_robot_id()
Detect this robot id
-* **Parameters**
-
- **id** – 0/1/2/3 (ALL/L/R/W)
-
### get_robot_version(id)
Get cobot version
@@ -1087,14 +1085,12 @@ Set the base coordinate system
* **rftype** – 0 - base 1 - tool.
-### set_robot_id(id, new_id)
+### set_robot_id(new_id)
Set this robot id
* **Parameters**
- * **id** – 0/1/2/3 (ALL/L/R/W)
-
* **new_id** – 1 - 253
### set_servo_calibration(id, servo_no)
diff --git a/docs/ultraArm_P340_en.md b/docs/ultraArm_P340_en.md
new file mode 100644
index 0000000..23bf69d
--- /dev/null
+++ b/docs/ultraArm_P340_en.md
@@ -0,0 +1,368 @@
+# Detailed Description of API Method
+
+**When using the following function interfaces, please import our API library at the beginning, otherwise the operation cannot succeed, that is, enter the following code:**
+
+```python
+#For Mira Series
+from pymycobot.ultraArmP340 import ultraArmP340
+```
+
+**Note：** If our API library is not installed, please refer to [Python environment setup](../4-Python/1-InstallingthePythonEnvironment.md) section to install。
+
+## 1 Overall operation state of mechanical arm
+
+**1.1** `go_zero()`
+
+- **Function：** The mechanical arm returns to zero.
+- **Return value:** None
+
+**1.2** `power_on()`
+
+- **Function：** All joints of the mechanical arm are powered on.
+- **Return value:** None
+
+**1.3** `release_all_servos()`
+
+- **Function:** Power failure of all joints of mechanical arm
+- **Return value:** None
+
+**1.4** `is_moving_end()`
+
+- **Function:** Robot movement end flag
+- **Return value:**
+ - `1`: Movement end
+ - `0`: Movement not end
+
+**1.5** `set_system_value(id, address, value, mode=None)`
+
+- **Function:** Set system parameters
+- **Parameter description:**
+ - `id`: `int`, Motor ID, 4 or 7
+ - `address`: `int`, Parameter register address, 7 ~ 69
+ - `value`: `int`, Corresponding register parameter value
+ - `mode`: `int`, 1 or 2, can be empty, default mode is 1
+ - `1`: setting range is 0-255, address 21 (P value) can be used
+ - `2`: setting value range is 0-65535, address 56 (setting position) can be used
+- **Return value:** None
+
+**1.6** `get_system_value(id, address,mode=None)`
+
+- **Function:** Read system parameters
+- **Parameter description:**
+ - `id`: `int`, Motor ID, 4 or 7
+ - `address`: `int`, Parameter register address, 0 ~ 69
+ - `mode`: `int`, 1 or 2, can be empty, default mode is 1
+ - `1`: read range is 0-255, address 21 (P value) can be used
+ - `2`: read value range is 0-65535, address 56 (read position) can be used
+- **Return value:** `int`, Corresponding register parameter value
+
+**1.7** `get_system_version()`
+
+- **Function:** Read the firmware major and minor versions
+
+- **Return value:** `float`, firmware version number
+
+**1.8** `get_modify_version()`
+
+- **Function:** Read the firmware modified version number
+
+- **Return value:** `int`, modified version number
+
+## 2 Input program control mode (MDI mode)
+
+ > Note：The limit of different series of mechanical arms is different, and the attitude value can be set is also different. For details, see the parameter introduction of the corresponding model
+
+**2.1** `get_angles_info()`
+
+- **Function:** Get the current angle of the mechanical arm.
+- **Return value:** `list` a list of floating point values representing the angles of all joints
+
+**2.2** `set_angle(id, degree, speed)`
+
+- **Function:** Send the specified single joint motion to the specified angle
+- **Parameter description:**
+ - `id`: Represents the joint of the mechanical arm. The three axes have three joints, which can be represented by the number 1-3.
+ - `degree`: Represents the angle of the joint
+
+ <table>
+ <tr>
+ <th>Joint Id</th>
+ <th>Range</th>
+ </tr>
+ <tr>
+ <td text-align: center>1</td>
+ <td>-150 ~ 170</td>
+ </tr>
+ <tr>
+ <td>2</td>
+ <td>-20 ~ 90</td>
+ </tr>
+ <tr>
+ <td>3</td>
+ <td>-5 ~ 110</td>
+ </tr>
+ <tr>
+ <td>4（Accessories）</td>
+ <td> -179 ~ 179</td>
+ </tr>
+
+ </table>
+
+ - `speed`：It represents the speed of the mechanical arm movement, ranging from 0 to 200 (unit: mm/s).
+- **Return value:** None
+
+**2.3** `set_angles(degrees, speed)`
+
+- **Function:** Send all angles to all joints of the mechanical arm
+- **Parameter description:**
+ - `degrees`: (List[float]) include angles of all joints ,the three-axis robot has three joints, so the length is 3, and the representation method is: [20, 20, 20]
+ - `speed`: It represents the speed of the mechanical arm movement, and the value range is 0-200 (unit: mm/s).
+- **Return value:** None
+
+**2.4** `get_coords_info()`
+
+- **Function:** Get the current coordinates of the mechanical arm.
+- **Return value:** `list` include list of coordinates, `θ` is Rotation angle at the end
+ - Triaxial：The length is 3, which is in turn ` [x, y, z, θ]`
+
+
+**2.5** `set_coord(id,coord,speed)`
+
+- **Function:** Send a single coordinate value to the mechanical arm for movement
+- **Parameter description:**
+ - `id`:Represents the coordinates of the mechanical arm. The three axes have three coordinates and have a specific representation method.
+ Representation of X coordinate:`"X"or"x"`.
+ - `coord`: Enter the coordinate value you want to reach
+
+ <table>
+ <tr>
+ <th>Coord Id</th>
+ <th>Range</th>
+ </tr>
+ <tr>
+ <td text-align: center>x</td>
+ <td>-360 ~ 365.55</td>
+ </tr>
+ <tr>
+ <td>y</td>
+ <td>-365.55 ~ 365.55</td>
+ </tr>
+ <tr>
+ <td>z</td>
+ <td>-140 ~ 130</td>
+ </tr>
+
+ </table>
+
+ - `speed`: It represents the speed of the mechanical arm movement, and the range is 0-200 (unit: mm/s).
+- **Return value:** None
+
+**2.6** `set_coords(coords, speed)`
+
+- **Function:** Send the overall coordinates so that the head of the mechanical arm moves from the original point to the point you specify.
+- **Parameter description:**
+ - `coords`:
+ - Triaxial：The coordinate value of [x, y, z], with a length of 3
+ - `speed`: It represents the speed of the mechanical arm movement, and the range is 0-200 (unit: mm/s).
+- **Return value:** None
+
+**2.7** `get_radians_info()`
+
+- **Function：** Get the current radian value of the mechanical arm.
+- **Return value:** `List ` list containing all joint radian values
+
+**2.8** `set_radians(radians, speed)`
+
+- **Function:** Send radian values to all joints of the mechanical arm
+- **Parameter Description**
+ - `radians`: List of radian values for each joint( `List[float]`)
+
+ <table>
+ <tr>
+ <th>Joint Id</th>
+ <th>Range</th>
+ </tr>
+ <tr>
+ <td text-align: center>1</td>
+ <td>2.6179 ~ 2.9670</td>
+ </tr>
+ <tr>
+ <td>2</td>
+ <td>-0.3490 ~ 1.5707</td>
+ </tr>
+ <tr>
+ <td>3</td>
+ <td>-0.0872 ~ 1.9198</td>
+ </tr>
+ <tr>
+ <td>4 (Accessories)</td>
+ <td> -3.1241 ~ + 3.1241</td>
+ </tr>
+ </table>
+
+ - `speed`: It represents the speed of the mechanical arm movement, and the range is 0-200 (unit: mm/s).
+- **Return value:** None
+
+**2.9** `set_mode()`
+
+- **Function:** Set Coordinate Mode
+- **Parameter Description**
+ - `0`:Absolute Cartesian mode
+ - `1`:Relative Cartesian mode
+- **Return value:** None
+
+**2.10** `sleep()`
+
+- **Function:** Delay
+- **Parameter Description**
+ - `Time`: Time delayed( `Int`type)，
+- **Return value:** None
+
+**2.12** `set_init_pose()`
+
+- **Function:** Set the current position as a fixed position,for example（0,0,0）,set this position to zero
+- **Parameter Description**
+ - `coords`: All coordinates of mechanical arm
+ - `speed`: It represents the speed of the mechanical arm movement, and the range is 0-200 (unit: mm/s).
+- **Return value:** None
+
+**2.13** `set_pwm()`
+
+- **Function:** Set PWM duty cycle
+- **Parameter description:** P：Duty cycle，Range: 0-255
+- **Return value:** None
+
+**2.14** `set_fan_state()`
+
+- **Function:** Set Fan Status
+- **Parameter Description**
+ - `0`: close
+ - `1`: open
+- **Return value:** None
+
+
+**2.15** `get_switch_state()`
+
+- **Function:** Get limit switch status
+- **Parameter Description**
+ - `X`：Whether joint 3 reaches the limit position
+ - `Y`：Whether joint 2 reaches the limit position
+ - `Z`：Whether joint 1 reaches the limit position
+- **Return value:** Yes
+
+
+**2.16** `set_speed_mode(mode)`
+
+- **Function:** Set speed mode
+- **Parameter Description**
+ - `0`: Uniform speed mode
+ - `2`: Acceleration and deceleration mode
+- **Return value:** None
+
+
+
+## 3 JOG mode and operation
+
+**3.1** `set_jog_angle(id, direction, speed)`
+
+- **Function:** Set inching mode (angle)
+- **Parameter Description**
+ - `id`: Represents the joint of the mechanical arm, which is represented by 1~3 joint id
+ - `direction`: It mainly controls the moving direction of the machine arm. Input 0 is negative move and input 1 is positive move
+ - `speed`: Speed 0~200 (unit: mm/s)
+- **Return value:** None
+
+**3.2** `set_jog_coord(axis, direction, speed)`
+
+- **Function:** Control the robot to move continuously according to the specified coordinates or attitude values
+- **Parameter Description**
+ - `axis`: Represents the joint of the mechanical arm, which is represented by the x/y/z given by the axis of the joint
+ - `direction`: It mainly controls the moving direction of the machine arm. Input 0 is negative move and input 1 is positive move
+ - `speed`: Speed 0~200 (unit: mm/s)
+- **Return value:** None
+
+**3.3** `set_jog_stop()`
+
+- **Function:** Stop continuous movement under the control of jog
+- **Return value:** None
+
+
+
+## 4 IO control
+
+**4.1** `set_gpio_state(state)`
+
+- **Function:** Set suction pump status
+- **Parameter Description**
+ - `state` (int)：Input 0 indicates that the suction pump is started, and input 1 indicates that the suction pump is closed
+- **Return value:** None
+
+**4.2** `set_gripper_zero()`
+
+- **Function:** Set gripper zero position（Set the current position to zero）
+- **Return value:** None
+
+**4.3** `set_gripper_state(state):`
+
+- **Function:** Set gripper switch
+- **Parameter Description** `state`：Input 0 to open the gripper, and input 1 to close the gripper
+- **Return value:** None
+
+**4.4** `get_gripper_angle():`
+
+- **Function:** Get the gripper angle
+- **Return value:** Gripper angle value
+
+## 5 Functional interface
+
+**5.1** `play_gcode_file(filename)`
+
+- **Function:** Play imported track files
+- **Parameter description:**
+ - `filename` ：Track file name
+- **Return value:** None
+
+
+## 6 Use Cases
+
+```python
+import time
+import platform
+from pymycobot.ultraArmP340 import ultraArmP340
+
+# Automatically select the system and connect the mechanical arm
+if platform.system() == "Windows":
+ ua = ultraArmP340('COM6', 115200)
+ ua.go_zero()
+elif platform.system() == "Linux":
+ ua = ultraArmP340('/dev/ttyUSB0', 115200)
+ ua.go_zero()
+
+# Position of mechanical arm movement
+angles = [
+ [-81.71, 0.0, 0.0],
+ [-90.53, 21.77, 47.56],
+ [-90.53, 0.0, 0.0],
+ [-59.01, 21.77, 45.84],
+ [-59.01, 0.0, 0.0]
+]
+
+# Suck small pieces
+ua.set_angles(angles[0], 50)
+time.sleep(3)
+ua.set_angles(angles[1], 50)
+time.sleep(3)
+
+# Start suction pump
+ua.set_gpio_state(0)
+
+ua.set_angles(angles[2], 50)
+time.sleep(3)
+ua.set_angles(angles[3], 50)
+
+# Close the suction pump
+ua.set_gpio_state(1)
+time.sleep(3)
+
+ua.set_angles(angles[4], 50)
+```
diff --git a/docs/ultraArm_P340_zh.md b/docs/ultraArm_P340_zh.md
new file mode 100644
index 0000000..72d2313
--- /dev/null
+++ b/docs/ultraArm_P340_zh.md
@@ -0,0 +1,367 @@
+# API 方法详细说明
+
+**在使用下列函数接口的时候请先在开头导入我们的API库，否则无法运行成功，即输入以下代码：**
+
+```python
+from pymycobot.ultraArmP340 import ultraArmP340
+```
+
+**注意：** 若没有安装我们的API库，请参考 [README.md](../README.md) 文档进行安装。
+
+## 1 机械臂整体运行状态
+
+**1.1** `go_zero()`
+
+- **功能：** 机械臂回零。
+- **返回值：** 无
+
+**1.2** `power_on()`
+
+- **功能：** 机械臂所有关节上电。
+- **返回值：** 无
+
+**1.3** `release_all_servos()`
+
+- **功能：** 机械臂所有关节掉电
+- **返回值：** 无
+
+**1.4** `is_moving_end()`
+
+- **功能：** 机械臂运动结束标志
+- **返回值：**
+ - `1`: 运动结束
+ - `0`: 运动未结束
+
+**1.5** `set_system_value(id, address, value, mode=None)`
+
+- **功能：** 设置系统参数
+- **参数说明：**
+ - `id`: `int`, 电机ID，4 或者 7
+ - `address`: `int`, 参数寄存器地址，7 ~ 69
+ - `value`: `int`, 对应寄存器参数取值
+ - `mode`: `int`, 1 或者 2，可以为空，默认模式为1
+ - `1`: 设置范围为0-255，可使用地址21（P值）
+ - `2`: 设置取值范围0-65535，可使用地址56（设置位置）
+- **返回值：** 无
+
+**1.6** `get_system_value(id, address, mode=None)`
+
+- **功能：** 读取系统参数
+- **参数说明：**
+ - `id`: `int`, 电机ID，4 或者 7
+ - `address`: `int`, 参数寄存器地址，0 ~ 69
+ - `mode`: `int`, 1 或者 2，可以为空，默认模式为1
+ - `1`: 读取范围为0-255，可使用地址21（P值）
+ - `2`: 读取取值范围0-65535，可使用地址56（读取位置）
+- **返回值：** `int`, 对应寄存器参数取值
+
+**1.7** `get_system_version()`
+
+- **功能：** 读取固件主次版本
+
+- **返回值：** `float`, 固件版本号
+
+**1.8** `get_modify_version()`
+
+- **功能：** 读取固件更正版本号
+
+- **返回值：** `int`, 更正版本号
+
+## 2 输入程序控制模式（MDI模式）
+
+ > 注意：不同系列的机械臂限位有所不同，可设定姿态数值也有所不同具体可查看对应型号的参数介绍
+
+**2.1** `get_angles_info()`
+
+- **功能：** 获取机械臂当前角度。
+- **返回值：** `list`一个浮点值的列表，表示所有关节的角度.
+
+**2.2** `set_angle(id, degree, speed)`
+
+- **功能：** 发送指定的单个关节运动至指定的角度
+- **参数说明：**
+ - `id`: 代表机械臂的关节，三轴有三个关节，可以用数字1-3来表示。
+ - `degree`: 表示关节的角度
+
+ <table>
+ <tr>
+ <th>关节 Id</th>
+ <th>范围</th>
+ </tr>
+ <tr>
+ <td text-align: center>1</td>
+ <td>-150 ~ 170</td>
+ </tr>
+ <tr>
+ <td>2</td>
+ <td>-20 ~ 90</td>
+ </tr>
+ <tr>
+ <td>3</td>
+ <td>-5 ~ 110</td>
+ </tr>
+ <tr>
+ <td>4（配件）</td>
+ <td> -179 ~ 179</td>
+ </tr>
+
+ </table>
+
+ - `speed`：表示机械臂运动的速度，范围0~200 (单位：mm/s)。
+- **返回值：** 无
+
+**2.3** `set_angles(degrees, speed)`
+
+- **功能：** 发送所有角度给机械臂所有关节
+- **参数说明：**
+ - `degrees`: (List[float])包含所有关节的角度 ,三轴机器人有三个关节，所以长度为3，表示方法为：[20,20,20]
+ - `speed`: 表示机械臂运动的速度，取值范围是0~200 (单位：mm/s)。
+- **返回值：** 无
+
+**2.4** `get_coords_info()`
+
+- **功能：** 获取机械臂当前坐标。
+- **返回值：** `list`包含坐标的列表, `θ` 为末端的旋转角
+ - 三轴：长度为 4，依次为 `[x, y, z, θ]`
+
+
+**2.5** `set_coord(id,coord,speed)`
+
+- **功能：** 发送单个坐标值给机械臂进行移动
+- **参数说明：**
+ - `id`:代表机械臂的坐标，三轴有三个坐标，有特定的表示方法。
+ X坐标的表示法：`"X"或者"x"`.
+ - `coord`: 输入您想要到达的坐标值
+
+ <table>
+ <tr>
+ <th>坐标 Id</th>
+ <th>范围</th>
+ </tr>
+ <tr>
+ <td text-align: center>x</td>
+ <td>-360 ~ 365.55</td>
+ </tr>
+ <tr>
+ <td>y</td>
+ <td>-365.55 ~ 365.55</td>
+ </tr>
+ <tr>
+ <td>z</td>
+ <td>-140 ~ 130</td>
+ </tr>
+
+ </table>
+
+ - `speed`: 表示机械臂运动的速度，范围是0-200 (单位：mm/s)。
+- **返回值：** 无
+
+**2.6** `set_coords(coords, speed)`
+
+- **功能：** 发送整体坐标,让机械臂头部从原来点移动到您指定点。
+- **参数说明：**
+ - `coords`:
+ - 三轴：[x,y,z] 的坐标值，长度为3
+ - `speed`: 表示机械臂运动的速度，范围是0-200 (单位：mm/s)。
+- **返回值：** 无
+
+**2.7** `get_radians_info()`
+
+- **功能：** 获取机械臂当前弧度值。
+- **返回值：** `list`包含所有关节弧度值的列表.
+
+**2.8** `set_radians(radians, speed)`
+
+- **功能：** 发送弧度值给机械臂所有关节
+- **参数说明：**
+ - `radians`: 每个关节的弧度值列表( `List[float]`)
+
+ <table>
+ <tr>
+ <th>关节 Id</th>
+ <th>范围</th>
+ </tr>
+ <tr>
+ <td text-align: center>1</td>
+ <td>2.6179 ~ 2.9670</td>
+ </tr>
+ <tr>
+ <td>2</td>
+ <td>-0.3490 ~ 1.5707</td>
+ </tr>
+ <tr>
+ <td>3</td>
+ <td>-0.0872 ~ 1.9198</td>
+ </tr>
+ <tr>
+ <td>4（配件）</td>
+ <td> -3.1241 ~ + 3.1241</td>
+ </tr>
+ </table>
+
+ - `speed`: 表示机械臂运动的速度，范围是0-200 (单位：mm/s)。
+- **返回值：** 无
+
+**2.9** `set_mode()`
+
+- **功能：** 设置坐标模式
+- **参数说明：**
+ - `0`:绝对笛卡尔模式。
+ - `1`:相对笛卡尔模式。
+- **返回值：** 无
+
+**2.10** `sleep()`
+
+- **功能：** 延迟
+- **参数说明：**
+ - `Time`: 延迟的时间( `Int`类型)，
+- **返回值：** 无
+
+**2.12** `set_init_pose()`
+
+- **功能：** 设置当前位置为某个固定位置。如（0,0,0）就把这个位置设为零点
+- **参数说明：**
+ - `coords`: 机械臂所有坐标
+ - `speed`: 表示机械臂运动的速度，范围是0-200 (单位：mm/s)。
+- **返回值：** 无
+
+**2.13** `set_pwm()`
+
+- **功能：** 设置PWM占空比
+- **参数说明：** P：占空比，范围：0-255
+- **返回值：** 无
+
+**2.14** `set_fan_state()`
+
+- **功能：** 设置风扇状态
+- **参数说明：**
+ - `0`: close
+ - `1`: open
+- **返回值：** 无
+
+
+**2.15** `get_switch_state()`
+
+- **功能：** 获取限位开关状态
+- **参数说明：**
+ - `X`：关节三是否到达限位
+ - `Y`：关节二是否到达限位
+ - `Z`：关节一是否到达限位
+- **返回值：** 有
+
+
+**2.16** `set_speed_mode(mode)`
+
+- **功能：** 设置速度模式
+- **参数说明：**
+ - `0`: 匀速模式
+ - `2`: 加减速模式
+- **返回值：** 无
+
+
+
+## 3 JOG 模式和操作
+
+**3.1** `set_jog_angle(id, direction, speed)`
+
+- **功能：** 设置点动模式（角度）
+- **参数说明：**
+ - `id`: 代表机械臂的关节，按照关节id给入1~3来表示
+ - `direction`: 主要控制机器臂移动的方向，给入0 为负值移动，给入1为正值移动
+ - `speed`: 速度 0 ~ 200 (单位：mm/s)。
+- **返回值：** 无
+
+**3.2** `set_jog_coord(axis, direction, speed)`
+
+- **功能：** 控制机器人按照指定的坐标或姿态值持续移动
+- **参数说明：**
+ - `axis`: 代表机械臂的关节，按照关节axis给入x/y/z来表示
+ - `direction`: 主要控制机器臂移动的方向，给入0 为负值移动，给入1为正值移动
+ - `speed`: 速度 0 ~ 200 (单位：mm/s)。
+- **返回值：** 无
+
+**3.3** `set_jog_stop()`
+
+- **功能：** 停止 jog 控制下的持续移动
+- **返回值：** 无
+
+
+
+## 4 IO控制
+
+**4.1** `set_gpio_state(state)`
+
+- **功能：** 设置 吸泵状态。
+- **参数说明：**
+ - `state` (int)：输入0表示开启吸泵，输入1表示关闭吸泵。
+- **返回值：** 无
+
+**4.2** `set_gripper_zero()`
+
+- **功能：** 设置夹爪零位（设置当前位置为零位）。
+- **返回值：** 无
+
+**4.3** `set_gripper_state(state):`
+
+- **功能：** 设置夹爪开关
+- **参数说明:** `state`：输入0表示张开夹爪，输入1表示闭合夹爪。
+- **返回值：** 无
+
+**4.4** `get_gripper_angle():`
+
+- **功能：** 获取夹爪角度
+- **返回值：** 夹爪角度值
+
+## 5 功能接口
+
+**5.1** `play_gcode_file(filename)`
+
+- **功能：** 播放导入的轨迹文件。
+- **参数说明：**
+ - `filename` ：轨迹文件名称
+- **返回值：** 无
+
+
+## 6 使用案例
+
+```python
+import time
+import platform
+from pymycobot.ultraArmP340 import ultraArmP340
+
+# 自动选择系统并连接机械臂
+if platform.system() == "Windows":
+ ua = ultraArmP340('COM6', 115200)
+ ua.go_zero()
+elif platform.system() == "Linux":
+ ua = ultraArmP340('/dev/ttyUSB0', 115200)
+ ua.go_zero()
+
+# 机械臂运动的位置
+angles = [
+ [-81.71, 0.0, 0.0],
+ [-90.53, 21.77, 47.56],
+ [-90.53, 0.0, 0.0],
+ [-59.01, 21.77, 45.84],
+ [-59.01, 0.0, 0.0]
+]
+
+# 吸取小物块
+ua.set_angles(angles[0], 50)
+time.sleep(3)
+ua.set_angles(angles[1], 50)
+time.sleep(3)
+
+# 开启吸泵
+ua.set_gpio_state(0)
+
+ua.set_angles(angles[2], 50)
+time.sleep(3)
+ua.set_angles(angles[3], 50)
+
+# 关闭吸泵
+ua.set_gpio_state(1)
+time.sleep(3)
+
+ua.set_angles(angles[4], 50)
+```
diff --git a/pymycobot/Interface.py b/pymycobot/Interface.py
index f1ffbf0..00667a7 100644
--- a/pymycobot/Interface.py
+++ b/pymycobot/Interface.py
@@ -59,14 +59,13 @@ def get_robot_id(self):
"""
return self._mesg(ProtocolCode.GET_ROBOT_ID, 0, has_reply=True)
- def set_robot_id(self, id, new_id):
+ def set_robot_id(self, new_id):
"""Set this robot id
Args:
- id: 0/1/2/3 (ALL/L/R/W)
new_id: 1 - 253
"""
- return self._mesg(ProtocolCode.SET_ROBOT_ID, id, new_id)
+ return self._mesg(ProtocolCode.SET_ROBOT_ID, new_id)
# Overall status
def power_on(self, id=0):
diff --git a/pymycobot/__init__.py b/pymycobot/__init__.py
index 647719f..4aed61a 100644
--- a/pymycobot/__init__.py
+++ b/pymycobot/__init__.py
@@ -4,12 +4,12 @@
import datetime
import sys
from pymycobot.mycobot280 import MyCobot280
+from pymycobot.mycobot280rdkx5 import MyCobot280RDKX5, MyCobot280RDKX5Socket
from pymycobot.mypalletizer260 import MyPalletizer260
from pymycobot.mecharm270 import MechArm270
from pymycobot.mycobot280socket import MyCobot280Socket
from pymycobot.mycobot320socket import MyCobot320Socket
from pymycobot.mycobot320 import MyCobot320
-from pymycobot.ultraArmP340 import ultraArmP340
from pymycobot.generate import CommandGenerator
from pymycobot.Interface import MyBuddyCommandGenerator
from pymycobot.mycobot import MyCobot
@@ -25,12 +25,12 @@
from pymycobot.mypalletizersocket import MyPalletizerSocket
from pymycobot.myarm import MyArm
from pymycobot.myarmsocket import MyArmSocket
-from pymycobot.mycobotpro630 import Phoenix
from pymycobot.elephantrobot import ElephantRobot
from pymycobot.mercury import Mercury
from pymycobot.myagv import MyAgv
+from pymycobot.myarmsocket import MyArmSocket
from pymycobot.mecharmsocket import MechArmSocket
-from pymycobot.mercurychassis import MercuryChassis
+from pymycobot.mycobotpro630 import Phoenix
from pymycobot.mercurysocket import MercurySocket
from pymycobot.myarmm import MyArmM
from pymycobot.myarmc import MyArmC
@@ -39,13 +39,14 @@
from pymycobot.pro400 import Pro400
from pymycobot.pro400client import Pro400Client
from pymycobot.myarmm_control import MyArmMControl
-
+from pymycobot.mercurychassis_api import ChassisControl
+from pymycobot.conveyor_api import ConveyorAPI
+from pymycobot.ultraArmP340 import ultraArmP340
__all__ = [
"MyPalletizer260",
"MechArm270",
"MyCobot280",
"MyCobot320",
- "ultraArmP340",
"MyCobot",
"CommandGenerator",
"Angle",
@@ -66,7 +67,6 @@
"MyAgv",
"MechArmSocket",
"MyArmSocket",
- "MercuryChassis",
"MercurySocket",
"MyArmM",
"MyArmC",
@@ -77,15 +77,19 @@
"Pro400Client",
"MyCobot280Socket",
"MyCobot320Socket",
- "MyArmMControl"
-
+ "MyArmMControl",
+ "ChassisControl",
+ "ConveyorAPI",
+ "MyCobot280RDKX5",
+ "MyCobot280RDKX5Socket",
+ "ultraArmP340"
]
if sys.platform == "linux":
from pymycobot.mybuddyemoticon import MyBuddyEmoticon
__all__.append("MyBuddyEmoticon")
-__version__ = "3.6.2"
+__version__ = "3.9.2b3"
__author__ = "Elephantrobotics"
__email__ = "weiquan.xu@elephantrobotics.com"
__git_url__ = "https://github.com/elephantrobotics/pymycobot"
diff --git a/pymycobot/close_loop.py b/pymycobot/close_loop.py
index e943a4a..d6b67fb 100644
--- a/pymycobot/close_loop.py
+++ b/pymycobot/close_loop.py
@@ -1,14 +1,18 @@
# coding=utf-8
import time
import struct
+from datetime import datetime
+import re
from pymycobot.error import calibration_parameters
-from pymycobot.generate import CommandGenerator
-from pymycobot.common import ProtocolCode, write, read
+from pymycobot.common import ProtocolCode, write, read, DataProcessor, FingerGripper
+from pymycobot.end_control import ForceGripper, ThreeHand
-class CloseLoop(CommandGenerator):
+
+class CloseLoop(DataProcessor, ForceGripper, ThreeHand):
_write = write
_read = read
+
def __init__(self, debug=False):
super(CloseLoop, self).__init__(debug)
self.calibration_parameters = calibration_parameters
@@ -16,7 +20,17 @@ def __init__(self, debug=False):
self.write_command = []
self.read_command = []
self.no_return = False
-
+ self.sync_mode = True
+ self.all_debug_data = []
+ self.all_read_data = b""
+
+ def _send_command(self, genre, real_command):
+ self.write_command.append(genre)
+ if "Socket" in self.__class__.__name__ or "Client" in self.__class__.__name__:
+ self._write(self._flatten(real_command), method="socket")
+ else:
+ self._write(self._flatten(real_command))
+
def _mesg(self, genre, *args, **kwargs):
"""
@@ -29,84 +43,337 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(CloseLoop, self)._mesg(genre, *args, **kwargs)
- no_return = kwargs.get("no_return", False)
- if no_return:
- self.no_return = True
- t = kwargs.get("asyn_mode", None)
- if t:
- self.no_return = True
- elif t == False:
- self.no_return = False
- # is_in_position = False
+ real_command, has_reply, _async = super(
+ CloseLoop, self)._mesg(genre, *args, **kwargs)
+ is_in_position = False
+ is_get_return = False
+ lost_times = 0
with self.lock:
- self.write_command.append(genre)
- if self.__class__.__name__ in ["Pro630Client", "Pro400Client"]:
- self._write(self._flatten(real_command), "socket")
- time.sleep(0.01)
- else:
- self._write(self._flatten(real_command))
-
- close_loop = [ProtocolCode.SEND_ANGLE, ProtocolCode.SEND_ANGLES, ProtocolCode.SEND_COORD, ProtocolCode.SEND_COORDS, ProtocolCode.JOG_INCREMENT, ProtocolCode.JOG_INCREMENT_COORD, ProtocolCode.COBOTX_SET_SOLUTION_ANGLES]
- if genre in close_loop and self.no_return == False:
- has_reply = True
- if genre == ProtocolCode.SEND_ANGLES and no_return:
- has_reply = False
- if has_reply:
- t = time.time()
- wait_time = 0.1
- if genre == ProtocolCode.POWER_ON:
- wait_time = 8
- elif genre in [ProtocolCode.POWER_OFF, ProtocolCode.RELEASE_ALL_SERVOS, ProtocolCode.FOCUS_ALL_SERVOS,
- ProtocolCode.RELEASE_SERVO, ProtocolCode.FOCUS_SERVO, ProtocolCode.STOP]:
- wait_time = 3
- # elif genre in close_loop:
- # wait_time = 300
- # is_in_position = True
- need_break = False
- data = None
- while True and time.time() - t < wait_time:
+ self._send_command(genre, real_command)
+ if _async:
+ with self.lock:
+ if genre in self.write_command:
+ self.write_command.remove(genre)
+ return None
+ t = time.time()
+ wait_time = 0.15
+ big_wait_time = False
+ if genre == ProtocolCode.POWER_ON:
+ wait_time = 8
+ big_wait_time = True
+ elif genre in [ProtocolCode.POWER_OFF, ProtocolCode.POWER_ON_ONLY, ProtocolCode.RELEASE_ALL_SERVOS,
+ ProtocolCode.FOCUS_ALL_SERVOS,
+ ProtocolCode.RELEASE_SERVO, ProtocolCode.FOCUS_SERVO, ProtocolCode.STOP,
+ ProtocolCode.SET_CONTROL_MODE, ProtocolCode.MERCURY_DRAG_TEACH_CLEAN]:
+ wait_time = 3
+ big_wait_time = True
+ elif genre in [
+ ProtocolCode.SEND_ANGLE,
+ ProtocolCode.SEND_ANGLES,
+ ProtocolCode.SEND_COORD,
+ ProtocolCode.SEND_COORDS,
+ ProtocolCode.JOG_ANGLE,
+ ProtocolCode.JOG_COORD,
+ ProtocolCode.JOG_INCREMENT,
+ ProtocolCode.JOG_INCREMENT_COORD,
+ ProtocolCode.COBOTX_SET_SOLUTION_ANGLES,
+ ProtocolCode.MERCURY_SET_BASE_COORDS,
+ ProtocolCode.MERCURY_JOG_BASE_COORD,
+ ProtocolCode.MERCURY_SET_BASE_COORD,
+ ProtocolCode.OVER_LIMIT_RETURN_ZERO,
+ ProtocolCode.JOG_BASE_INCREMENT_COORD,
+ ProtocolCode.WRITE_MOVE_C,
+ ProtocolCode.JOG_RPY,
+ ProtocolCode.WRITE_MOVE_C_R,
+ ProtocolCode.MERCURY_DRAG_TECH_EXECUTE] and self.sync_mode:
+ wait_time = 300
+ is_in_position = True
+ big_wait_time = True
+ elif genre in [ProtocolCode.SERVO_RESTORE]:
+ wait_time = 0.3
+ big_wait_time = True
+
+ elif genre in (ProtocolCode.MERCURY_SET_TOQUE_GRIPPER, ProtocolCode.MERCURY_GET_TOQUE_GRIPPER):
+ wait_time = 0.3
+ if real_command[3] == FingerGripper.SET_HAND_GRIPPER_CALIBRATION:
+ wait_time = 10
+
+ need_break = False
+ data = None
+ timeout = 0.5
+ if self.__class__.__name__ == "MercurySocket":
+ timeout = 1
+ interval_time = time.time()
+ is_moving = 0
+ check_is_moving_t = 1
+ while True and time.time() - t < wait_time:
+ with self.lock_out:
for v in self.read_command:
- # if is_in_position and v == b'\xfe\xfe\x04[\x01\r\x87' and genre in close_loop:
- # need_break = True
- # with self.lock:
- # self.read_command.remove(v)
- # self.write_command.remove(genre)
- # return 1
- # elif genre == v[3]:
- if genre == v[3]:
+ read_data = v[0]
+ if is_get_return and is_in_position and read_data[2] == 0x04 and read_data[3] == 0x5b:
+ if v[1] < t:
+ with self.lock:
+ if v in self.read_command:
+ self.read_command.remove(v)
+ continue
+ # print("到位反馈", flush=True)
+ is_get_return = True
+ need_break = True
+ data = read_data
+ with self.lock:
+ if v in self.read_command:
+ self.read_command.remove(v)
+ if genre in self.write_command:
+ self.write_command.remove(genre)
+
+ elif genre == read_data[3] and read_data[2] == 5 and read_data[4] == 0xFF:
+ # 通信闭环
+ # print(-2)
+ # print("闭环", flush=True)
+ is_get_return = True
+ with self.lock:
+ if v in self.read_command:
+ self.read_command.remove(v)
+ if has_reply == False or self.sync_mode == False:
+ # print(-3)
+ # print("仅闭环退出", flush=True)
+ need_break = True
+ data = read_data
+ elif genre == read_data[3]:
+ # print(-4)
+ # print("正常读取", flush=True)
need_break = True
- data = v
+ data = read_data
with self.lock:
- self.read_command.remove(v)
- self.write_command.remove(genre)
+ if v in self.read_command:
+ self.read_command.remove(v)
+ if genre in self.write_command:
+ self.write_command.remove(genre)
break
- if need_break:
- break
- time.sleep(0.01)
- if data is None:
- return data
- data = bytearray(data)
- data_len = data[2] - 3
- # unique_data = [ProtocolCode.GET_BASIC_INPUT, ProtocolCode.GET_DIGITAL_INPUT]
- if genre == ProtocolCode.GET_BASIC_INPUT:
- data_pos = 5
- data_len -= 1
- if self.__class__.__name__ == "Pro630Client":
- data_len += 1
- else:
- data_pos = 4
-
- valid_data = data[data_pos : data_pos + data_len]
- return (valid_data, data_len)
- return None
-
+ if (not big_wait_time or is_in_position) and not is_get_return and time.time() - t > timeout:
+ # 发送指令以后，超时0.5S没有收到第一次反馈，并且是运动指令，并且超时时间是正常指令
+ t = time.time()
+ moving = self.is_moving()
+ if isinstance(moving, int):
+ if moving != 0:
+ continue
+ # 运动指令丢失，重发
+ # print("运动指令丢失，重发", flush=True)
+ lost_times += 1
+ # print("运动指令丢失，重发")
+ with self.lock:
+ self._send_command(genre, real_command)
+ if need_break:
+ # print("正常退出", flush=True)
+ break
+ if lost_times > 2:
+ # 重传3次失败，返回-1
+ # print("重传3次失败，返回-1", flush=True)
+ return -1
+ # if t < self.is_stop and genre != ProtocolCode.STOP:
+ # # 打断除了stop指令外的其他指令的等待
+ # self.is_stop = 0
+ # break
+ if is_in_position and time.time() - interval_time > check_is_moving_t and wait_time == 300:
+ interval_time = time.time()
+ moving = self.is_moving()
+ if isinstance(moving, int) and moving == 0:
+ # print("停止运动，退出")
+ is_moving += 1
+ # 由于is_moving接口比到位反馈更快，所以第一次收到停止运动后，将下一次的检测时间更改为0.25s，防止此处先退出，返回-2
+ check_is_moving_t = 0.25
+ if is_moving > 1:
+ # 累计两次才退出
+ with self.lock:
+ if genre in self.write_command:
+ self.write_command.remove(genre)
+ return -2
+ time.sleep(0.001)
+ else:
+ # print("ERROR: ---超时---"
+ pass
+ if data is None:
+ # print("未拿到数据")
+ return data
+ data = bytearray(data)
+ data_len = data[2] - 3
+ # unique_data = [ProtocolCode.GET_BASIC_INPUT, ProtocolCode.GET_DIGITAL_INPUT]
+ if genre == ProtocolCode.GET_BASIC_INPUT:
+ data_pos = 5
+ data_len -= 1
+ if self.__class__.__name__ == "Pro630Client":
+ data_len += 1
+ else:
+ data_pos = 4
+ if is_get_return:
+ data_len -= 1
+ data_pos += 1
+ if data[2] == 5:
+ # print("握手成功")
+ return data[5]
+ elif data[2] == 4:
+ res = self._status_explain(data[4])
+ if res != "":
+ print(res)
+ return data[4]
+ valid_data = data[data_pos: data_pos + data_len]
+ return (valid_data, data_len)
+
+ def read_thread(self, method=None):
+ all_data = b''
+ while True:
+ try:
+ datas = b""
+ data_len = -1
+ k = 0
+ pre = 0
+ t = time.time()
+ wait_time = 0.5
+ if method is not None:
+ try:
+ self.sock.settimeout(wait_time)
+ data = self.sock.recv(1024)
+ if isinstance(data, str):
+ datas = bytearray()
+ for i in data:
+ datas += hex(ord(i))
+ data = datas
+ except:
+ data = b""
+ if data != b"":
+ pattern = re.compile(rb'###(.*?)###', re.DOTALL)
+ matches = pattern.findall(data)
+ for match in matches:
+ if self.check_python_version() == 2:
+ command_log = ""
+ for d in match:
+ command_log += hex(ord(d))[2:] + " "
+ self.log.debug(
+ "_read : {}".format(command_log))
+ # self.log.debug("_read: {}".format([hex(ord(d)) for d in data]))
+ else:
+ command_log = ""
+ for d in match:
+ command_log += hex(d)[2:] + " "
+ self.log.debug(
+ "_read : {}".format(command_log))
+ res = self._process_received(match)
+ if res != []:
+ with self.lock:
+ self.read_command.append(
+ [res, time.time()])
+ else:
+ while True and time.time() - t < wait_time:
+ if self._serial_port.isOpen() and self._serial_port.inWaiting() > 0:
+ data = self._serial_port.read()
+ if self.save_serial_log:
+ all_data += data
+ # self.log.info(all_read_data)
+ k += 1
+ # print(datas, flush=True)
+ if data_len == 3:
+ datas += data
+ crc = self._serial_port.read(2)
+ self.all_read_data += data
+ if self.crc_check(datas) == [v for v in crc]:
+ datas += crc
+ break
+ if data_len == 1 and data == b"\xfa":
+ datas += data
+ # if [i for i in datas] == command:
+ # datas = b''
+ # data_len = -1
+ # k = 0
+ # pre = 0
+ # continue
+ # break
+ elif len(datas) == 2:
+ data_len = struct.unpack("b", data)[0]
+ datas += data
+ elif len(datas) > 2 and data_len > 0:
+ datas += data
+ data_len -= 1
+ elif data == b"\xfe":
+ if datas == b"":
+ datas += data
+ pre = k
+ else:
+ if k - 1 == pre:
+ datas += data
+ else:
+ datas = b"\xfe"
+ pre = k
+ else:
+ time.sleep(0.001)
+ else:
+ datas = b''
+ if self.save_serial_log:
+ if all_data != b'':
+ current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S")
+ with open("all_log.txt", "a") as f:
+ f.write(str(current_time) +
+ str(all_data)[2:-1] + "\n")
+ all_data = b''
+ if datas != b'':
+ res = self._process_received(datas)
+ if self.check_python_version() == 2:
+ command_log = ""
+ for d in datas:
+ data = hex(d)[2:]
+ if len(data) != 2:
+ data = "0" + data
+ command_log += data + " "
+ self.log.debug("_read : {}".format(command_log))
+ else:
+ command_log = ""
+ for d in datas:
+ data = hex(d)[2:]
+ if len(data) != 2:
+ data = "0" + data
+ command_log += data + " "
+ self.log.debug("_read : {}".format(command_log))
+ if datas[3] == 0x5D:
+ debug_data = []
+ for i in range(4, 32, 4):
+ byte_value = int.from_bytes(
+ datas[i:i + 4], byteorder='big', signed=True)
+ debug_data.append(byte_value)
+ # self.log.info("SERVO_COMMAND : {}".format(debug_data))
+ with open("plotPos.txt", "a") as f:
+ f.write(str(debug_data) + "\n")
+ continue
+ elif datas[3] == 0x8E and datas[2] == 0x3B:
+ debug_data = []
+
+ for i in range(4, 60, 2):
+ if i < 40:
+ data = self._decode_int16(
+ datas[i:i + 2]) / 1000
+ else:
+ data = self._decode_int16(datas[i:i + 2])
+ debug_data.append(data)
+ self.all_debug_data.append(debug_data)
+
+ continue
+ if res == []:
+ # print("res is empty")
+ continue
+ # if datas[3] == 0x5b:
+ # print("等待加入到读取队列")
+ with self.lock:
+ self.read_command.append([res, time.time()])
+ # if datas[3] == 0x5b:
+ # print("加入到读取队列成功")
+
+ except Exception as e:
+ # self.log.error("read error: {}".format(traceback.format_exc()))
+ pass
+ time.sleep(0.001)
+
def _process_received(self, data):
if not data:
return []
- elif data == b'\xfe\xfe\x04[\x01\r\x87':
- return data
-
+
data = bytearray(data)
data_len = len(data)
# Get valid header: 0xfe0xfe
@@ -114,109 +381,13 @@ def _process_received(self, data):
while header_j < data_len - 4:
if self._is_frame_header(data, header_i, header_j):
cmd_id = data[header_i + 3]
- if cmd_id in self.write_command:
- break
+ if cmd_id in self.write_command or cmd_id == 0x5B:
+ return data
header_i += 1
header_j += 1
else:
return []
- return data
-
- def read_thread(self, method=None):
- while True:
-
- datas = b""
- data_len = -1
- k = 0
- pre = 0
- t = time.time()
- wait_time = 0.1
- if method is not None:
- try:
- wait_time = 0.1
- self.sock.settimeout(wait_time)
- data = self.sock.recv(1024)
-
- if isinstance(data, str):
- datas = bytearray()
- for i in data:
- datas += hex(ord(i))
- except:
- data = b""
-
- if data != b'':
- if self.check_python_version() == 2:
- command_log = ""
- for d in data:
- command_log += hex(ord(d))[2:] + " "
- self.log.debug("_read : {}".format(command_log))
- # self.log.debug("_read: {}".format([hex(ord(d)) for d in data]))
- else:
- command_log = ""
- for d in data:
- command_log += hex(d)[2:] + " "
- self.log.debug("_read : {}".format(command_log))
- res = self._process_received(data)
- if res != []:
- with self.lock:
- self.read_command.append(res)
- else:
- while True and time.time() - t < wait_time:
- if self._serial_port.inWaiting() > 0:
- data = self._serial_port.read()
- k += 1
- if data_len == 3:
- datas += data
- crc = self._serial_port.read(2)
- if self.crc_check(datas) == [v for v in crc]:
- datas+=crc
- break
- if data_len == 1 and data == b"\xfa":
- datas += data
- # if [i for i in datas] == command:
- # datas = b''
- # data_len = -1
- # k = 0
- # pre = 0
- # continue
- # break
- elif len(datas) == 2:
- data_len = struct.unpack("b", data)[0]
- datas += data
- elif len(datas) > 2 and data_len > 0:
- datas += data
- data_len -= 1
- elif data == b"\xfe":
- if datas == b"":
- datas += data
- pre = k
- else:
- if k - 1 == pre:
- datas += data
- else:
- datas = b"\xfe"
- pre = k
- else:
- datas = b''
- if datas:
- res = self._process_received(datas)
- if res != [] and res[3] == 0x5b:
- continue
- if self.check_python_version() == 2:
- command_log = ""
- for d in datas:
- command_log += hex(ord(d))[2:] + " "
- self.log.debug("_read : {}".format(command_log))
- else:
- command_log = ""
- for d in datas:
- command_log += hex(d)[2:] + " "
- self.log.debug("_read : {}".format(command_log))
- if res != []:
- with self.lock:
- self.read_command.append(res)
- # return datas
-
+
def bytes4_to_int(self, bytes4):
i = 0
res = []
@@ -227,15 +398,61 @@ def bytes4_to_int(self, bytes4):
data = bytes4[i:i+4]
for b in data:
byte_value = byte_value * 256 + b
-
+
if byte_value & (1 << 31):
byte_value -= 1 << (32)
else:
- byte_value = int.from_bytes(bytes4[i:i+4], byteorder='big', signed=True)
- i+=4
+ byte_value = int.from_bytes(
+ bytes4[i:i+4], byteorder='big', signed=True)
+ i += 4
res.append(byte_value)
return res
-
+
+ def get_atom_version(self):
+ """Get atom firmware version.
+
+ Returns:
+ float: version number.
+ """
+ return self._mesg(ProtocolCode.GET_ATOM_VERSION)
+
+ def is_power_on(self):
+ """Adjust robot arm status
+
+ Return:
+ 1 - power on
+ 0 - power off
+ -1 - error data
+ """
+ return self._mesg(ProtocolCode.IS_POWER_ON)
+
+ def get_coords(self, angles=None):
+ """Get the coords from robot arm, coordinate system based on base. The target angle can be passed in, and the coordinate position corresponding to the target angle can be obtained.
+
+ Args:
+ angles (list): The angles of six joints. e.g. [0, 0, 0, 0, 0, 0]
+
+ Return:
+ list : A float list of coord . [x, y, z, rx, ry, rz].
+
+ """
+ if angles is None:
+ return self._mesg(ProtocolCode.GET_COORDS)
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, angles=angles)
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles)
+
+ def is_paused(self):
+ """Judge whether the manipulator pauses or not.
+
+ Return:
+ 1 - paused
+ 0 - not paused
+ -1 - error
+ """
+ return self._mesg(ProtocolCode.IS_PAUSED)
+
def set_solution_angles(self, angle, speed):
"""Set zero space deflection angle value
@@ -247,15 +464,15 @@ def set_solution_angles(self, angle, speed):
class_name=self.__class__.__name__, speed=speed, solution_angle=angle
)
return self._mesg(
- ProtocolCode.COBOTX_SET_SOLUTION_ANGLES, [self._angle2int(angle)], speed
+ ProtocolCode.COBOTX_SET_SOLUTION_ANGLES, [self._angle2int(angle)], speed, has_reply=True
)
def get_solution_angles(self):
"""Get zero space deflection angle value"""
- return self._mesg(ProtocolCode.COBOTX_GET_SOLUTION_ANGLES, has_reply=True)
+ return self._mesg(ProtocolCode.COBOTX_GET_SOLUTION_ANGLES)
def write_move_c(self, transpoint, endpoint, speed):
- """_summary_
+ """Circular trajectory motion
Args:
transpoint (list): Arc passing point coordinates
@@ -271,31 +488,31 @@ def write_move_c(self, transpoint, endpoint, speed):
else:
start.append(self._angle2int(transpoint[index]))
end.append(self._angle2int(endpoint[index]))
- return self._mesg(ProtocolCode.WRITE_MOVE_C, start, end, speed)
+ return self._mesg(ProtocolCode.WRITE_MOVE_C, start, end, speed, has_reply=True)
def focus_all_servos(self):
"""Lock all joints"""
- return self._mesg(ProtocolCode.FOCUS_ALL_SERVOS, has_reply=True)
+ return self._mesg(ProtocolCode.FOCUS_ALL_SERVOS)
- def go_home(self, robot, speed=20):
+ def go_home(self, robot=1, speed=20, _async=False):
"""Control the machine to return to the zero position.
-
+
Args:
robot (int):
- 1 - Mercury A1
- 2 - Mercury B1 or X1
+ 1 - left arm
+ 2 - right arm
speed (int): 1 ~ 100
Return:
1 : All motors return to zero position.
0 : failed.
"""
if robot == 1:
- return self.sync_send_angles([0, 0, 0, 0, 0, 90, 0], speed)
+ return self.send_angles([0, 0, 0, 0, 0, 90, 0], speed, _async=_async)
else:
self.send_angle(11, 0, speed)
self.send_angle(12, 0, speed)
self.send_angle(13, 0, speed)
- return self.sync_send_angles([0, 0, 0, 0, 0, 90, 0], speed)
+ return self.send_angles([0, 0, 0, 0, 0, 90, 0], speed, _async=_async)
def get_angle(self, joint_id):
"""Get single joint angle
@@ -303,9 +520,18 @@ def get_angle(self, joint_id):
Args:
joint_id (int): 1 ~ 7 or 11 ~ 13.
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id)
- return self._mesg(ProtocolCode.COBOTX_GET_ANGLE, joint_id, has_reply=True)
-
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id)
+ return self._mesg(ProtocolCode.COBOTX_GET_ANGLE, joint_id)
+
+ def get_angles(self):
+ """ Get the angle of all joints.
+
+ Return:
+ list: A float list of all angle.
+ """
+ return self._mesg(ProtocolCode.GET_ANGLES)
+
def servo_restore(self, joint_id):
"""Abnormal recovery of joints
@@ -318,26 +544,26 @@ def servo_restore(self, joint_id):
self.calibration_parameters(
class_name=self.__class__.__name__, servo_restore=joint_id
)
- self._mesg(ProtocolCode.SERVO_RESTORE, joint_id)
-
- def set_error_detect_mode(self, mode):
- """Set error detection mode. Turn off without saving, default to open state
-
- Return:
- mode : 0 - close 1 - open.
- """
- self.calibration_parameters(
- class_name=self.__class__.__name__, mode=mode
- )
- self._mesg(ProtocolCode.SET_ERROR_DETECT_MODE, mode)
-
- def get_error_detect_mode(self):
- """Set error detection mode"""
- return self._mesg(ProtocolCode.GET_ERROR_DETECT_MODE, has_reply=True)
-
+ return self._mesg(ProtocolCode.SERVO_RESTORE, joint_id)
+
+ # def set_error_detect_mode(self, mode):
+ # """Set error detection mode. Turn off without saving, default to open state
+
+ # Return:
+ # mode : 0 - close 1 - open.
+ # """
+ # self.calibration_parameters(
+ # class_name=self.__class__.__name__, mode=mode
+ # )
+ # self._mesg(ProtocolCode.SET_ERROR_DETECT_MODE, mode)
+
+ # def get_error_detect_mode(self):
+ # """Set error detection mode"""
+ # return self._mesg(ProtocolCode.GET_ERROR_DETECT_MODE, has_reply=True)
+
def sync_send_angles(self, degrees, speed, timeout=15):
"""Send the angle in synchronous state and return when the target point is reached
-
+
Args:
degrees: a list of degree values(List[float]), length 6.
speed: (int) 0 ~ 100
@@ -352,92 +578,33 @@ def sync_send_angles(self, degrees, speed, timeout=15):
time.sleep(0.1)
return 0
- def sync_send_coords(self, coords, speed, mode=None, timeout=15):
- """Send the coord in synchronous state and return when the target point is reached
-
- Args:
- coords: a list of coord values(List[float])
- speed: (int) 0 ~ 100
- mode: (int): 0 - angular（default）, 1 - linear
- timeout: default 7s.
- """
- t = time.time()
- self.send_coords(coords, speed, mode)
- while time.time() - t < timeout:
- if self.is_in_position(coords, 1) == 1:
- return 1
- time.sleep(0.1)
- return 0
-
- def get_base_coords(self):
- """get base coords"""
- return self._mesg(ProtocolCode.MERCURY_GET_BASE_COORDS, has_reply=True)
-
- def send_base_coord(self, axis, coord, speed):
- """_summary_
-
- Args:
- axis (_type_): _description_
- coord (_type_): _description_
- speed (_type_): _description_
- """
- if axis < 4:
- coord = self._coord2int(coord)
- else:
- coord = self._angle2int(coord)
- return self._mesg(ProtocolCode.MERCURY_SET_BASE_COORD, axis, [coord], speed)
-
- def send_base_coords(self, coords, speed):
- """_summary_
-
- Args:
- coords (_type_): _description_
- speed (_type_): _description_
- """
- coord_list = []
- for idx in range(3):
- coord_list.append(self._coord2int(coords[idx]))
- for angle in coords[3:]:
- coord_list.append(self._angle2int(angle))
- return self._mesg(ProtocolCode.MERCURY_SET_BASE_COORDS, coord_list, speed)
-
- def jog_base_coord(self, axis, direction, speed):
- """_summary_
-
- Args:
- axis (_type_): _description_
- direction (_type_): _description_
- speed (_type_): _description_
- """
- return self._mesg(ProtocolCode.MERCURY_JOG_BASE_COORD, axis, direction, speed)
-
def drag_teach_save(self):
"""Start recording the dragging teaching point. In order to show the best sports effect, the recording time should not exceed 90 seconds."""
return self._mesg(ProtocolCode.MERCURY_DRAG_TECH_SAVE)
-
+
def drag_teach_execute(self):
"""Start dragging the teaching point and only execute it once."""
- return self._mesg(ProtocolCode.MERCURY_DRAG_TECH_EXECUTE)
-
+ return self._mesg(ProtocolCode.MERCURY_DRAG_TECH_EXECUTE, has_reply=True)
+
def drag_teach_pause(self):
"""Pause recording of dragging teaching point"""
- self._mesg(ProtocolCode.MERCURY_DRAG_TECH_PAUSE)
-
- def is_gripper_moving(self, mode=None):
- """Judge whether the gripper is moving or not
-
- Args:
- mode: 1 - pro gripper(default) 2 - Parallel gripper
+ return self._mesg(ProtocolCode.MERCURY_DRAG_TECH_PAUSE)
+
+ # def is_gripper_moving(self, mode=None):
+ # """Judge whether the gripper is moving or not
+
+ # Args:
+ # mode: 1 - pro gripper(default) 2 - Parallel gripper
+
+ # Returns:
+ # 0 - not moving
+ # 1 - is moving
+ # -1- error data
+ # """
+ # if mode:
+ # return self._mesg(ProtocolCode.IS_GRIPPER_MOVING, mode, has_reply=True)
+ # return self._mesg(ProtocolCode.IS_GRIPPER_MOVING, has_reply=True)
- Returns:
- 0 - not moving
- 1 - is moving
- -1- error data
- """
- if mode:
- return self._mesg(ProtocolCode.IS_GRIPPER_MOVING, mode, has_reply=True)
- return self._mesg(ProtocolCode.IS_GRIPPER_MOVING, has_reply=True)
-
def set_gripper_enabled(self, value):
"""Pro adaptive gripper enable setting
@@ -446,199 +613,222 @@ def set_gripper_enabled(self, value):
1 : enable
0 : release
"""
- self.calibration_parameters(class_name=self.__class__.__name__, value=value)
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, value=value)
return self._mesg(ProtocolCode.SET_GRIPPER_ENABLED, value)
-
+
def is_btn_clicked(self):
"""Check if the end button has been pressed.
-
+
Return:
1 : pressed.
0 : not pressed.
"""
- return self._mesg(ProtocolCode.IS_BTN_CLICKED, has_reply=True)
-
- def tool_serial_restore(self):
- """485 factory reset
- """
- return self._mesg(ProtocolCode.TOOL_SERIAL_RESTORE)
-
- def tool_serial_ready(self):
- """Set up 485 communication
-
- Return:
- 0 : not set
- 1 : Setup completed
- """
- return self._mesg(ProtocolCode.TOOL_SERIAL_READY, has_reply=True)
-
- def tool_serial_available(self):
- """Read 485 buffer length
-
- Return:
- 485 buffer length available for reading
- """
- return self._mesg(ProtocolCode.TOOL_SERIAL_AVAILABLE, has_reply=True)
-
- def tool_serial_read_data(self, data_len):
- """Read fixed length data. Before reading, read the buffer length first. After reading, the data will be cleared
+ return self._mesg(ProtocolCode.IS_BTN_CLICKED)
- Args:
- data_len (int): The number of bytes to be read, range 1 ~ 45
- """
- self.calibration_parameters(class_name=self.__class__.__name__, data_len=data_len)
- return self._mesg(ProtocolCode.TOOL_SERIAL_READ_DATA, data_len, has_reply=True)
-
- def tool_serial_write_data(self, command):
- """End 485 sends data， Data length range is 1 ~ 45 bytes
+ # def tool_serial_restore(self):
+ # """485 factory reset
+ # """
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_RESTORE)
- Args:
- command : data instructions
-
- Return:
- number of bytes received
- """
- return self._mesg(ProtocolCode.TOOL_SERIAL_WRITE_DATA, command, has_reply=True)
-
- def tool_serial_flush(self):
- """Clear 485 buffer
- """
- return self._mesg(ProtocolCode.TOOL_SERIAL_FLUSH)
-
- def tool_serial_peek(self):
- """View the first data in the buffer, the data will not be cleared
-
- Return:
- 1 byte data
- """
- return self._mesg(ProtocolCode.TOOL_SERIAL_PEEK, has_reply=True)
-
- def tool_serial_set_baud(self, baud=115200):
- """Set 485 baud rate, default 115200
+ # def tool_serial_ready(self):
+ # """Set up 485 communication
- Args:
- baud (int): baud rate
- """
- return self._mesg(ProtocolCode.TOOL_SERIAL_SET_BAUD, baud)
-
- def tool_serial_set_timeout(self, max_time):
- """Set 485 timeout in milliseconds, default 30ms
+ # Return:
+ # 0 : not set
+ # 1 : Setup completed
+ # """
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_READY, has_reply=True)
- Args:
- max_time (int): timeout
- """
- self.calibration_parameters(class_name=self.__class__.__name__, max_time=max_time)
- return self._mesg(ProtocolCode.TOOL_SERIAL_SET_TIME_OUT, max_time)
+ # def tool_serial_available(self):
+ # """Read 485 buffer length
+
+ # Return:
+ # 485 buffer length available for reading
+ # """
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_AVAILABLE, has_reply=True)
+
+ # def tool_serial_read_data(self, data_len):
+ # """Read fixed length data. Before reading, read the buffer length first. After reading, the data will be cleared
+
+ # Args:
+ # data_len (int): The number of bytes to be read, range 1 ~ 45
+ # """
+ # self.calibration_parameters(
+ # class_name=self.__class__.__name__, data_len=data_len)
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_READ_DATA, data_len, has_reply=True)
+
+ # def tool_serial_write_data(self, command):
+ # """End 485 sends data， Data length range is 1 ~ 45 bytes
+
+ # Args:
+ # command : data instructions
+
+ # Return:
+ # number of bytes received
+ # """
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_WRITE_DATA, command, has_reply=True)
+
+ # def tool_serial_flush(self):
+ # """Clear 485 buffer
+ # """
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_FLUSH)
+
+ # def tool_serial_peek(self):
+ # """View the first data in the buffer, the data will not be cleared
+
+ # Return:
+ # 1 byte data
+ # """
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_PEEK, has_reply=True)
+
+ # def tool_serial_set_baud(self, baud=115200):
+ # """Set 485 baud rate, default 115200
+
+ # Args:
+ # baud (int): baud rate
+ # """
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_SET_BAUD, baud)
+
+ # def tool_serial_set_timeout(self, max_time):
+ # """Set 485 timeout in milliseconds, default 30ms
+
+ # Args:
+ # max_time (int): timeout
+ # """
+ # self.calibration_parameters(
+ # class_name=self.__class__.__name__, max_time=max_time)
+ # return self._mesg(ProtocolCode.TOOL_SERIAL_SET_TIME_OUT, max_time)
def get_robot_status(self):
- return self._mesg(ProtocolCode.MERCURY_ROBOT_STATUS, has_reply=True)
-
- def power_on(self):
- """Open communication with Atom."""
- return self._mesg(ProtocolCode.POWER_ON, has_reply=True)
+ return self._mesg(ProtocolCode.MERCURY_ROBOT_STATUS)
+
+ def power_on(self, mode=1):
+ """Power on the robot
+
+ Args:
+ mode (int, optional): 1 - Motor enable, 0 - The motor is not enabled, only initialized.
+
+ Return:
+ 1: success
+ 2: failed
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, mode=mode)
+ res = self._mesg(ProtocolCode.POWER_ON, mode)
+ if res == 1:
+ self.get_limit_switch()
+ time.sleep(1)
+ return res
def power_off(self):
- """Close communication with Atom."""
- return self._mesg(ProtocolCode.POWER_OFF, has_reply=True)
-
+ """Robot power outage"""
+ with self.lock:
+ self.read_command.clear()
+ return self._mesg(ProtocolCode.POWER_OFF)
+
def release_all_servos(self):
"""Relax all joints
"""
- return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS, has_reply=True)
-
- def focus_servo(self, servo_id):
+ return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS)
+
+ def focus_servo(self, joint_id):
"""Power on designated servo
Args:
- servo_id: int. joint id 1 - 7
+ joint_id: int. joint id 1 - 7
"""
- self.calibration_parameters(class_name = self.__class__.__name__, id=servo_id)
- return self._mesg(ProtocolCode.FOCUS_SERVO, servo_id, has_reply=True)
-
- def release_servo(self, servo_id):
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id)
+ return self._mesg(ProtocolCode.FOCUS_SERVO, joint_id)
+
+ def release_servo(self, joint_id):
"""Power off designated servo
Args:
- servo_id: int. joint id 1 - 7
+ joint_id: int. joint id 1 - 7
"""
- self.calibration_parameters(class_name = self.__class__.__name__, id=servo_id)
- return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id, has_reply=True)
-
- def stop(self):
- """Stop moving"""
- # self.is_stop = True
- # self.write_command.remove()
- return self._mesg(ProtocolCode.STOP, has_reply=True)
-
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id)
+ return self._mesg(ProtocolCode.RELEASE_SERVO, joint_id)
+
def get_robot_type(self):
"""Get robot type
"""
- return self._mesg(ProtocolCode.GET_ROBOT_ID, has_reply=True)
-
-
+ return self._mesg(ProtocolCode.GET_ROBOT_ID)
+
def get_zero_pos(self):
"""Read the zero encoder value
Returns:
list: The values of the zero encoders of the seven joints
"""
- return self._mesg(ProtocolCode.GET_ZERO_POS, has_reply=True)
-
+ return self._mesg(ProtocolCode.GET_ZERO_POS)
+
def is_init_calibration(self):
"""Check if the robot is initialized for calibration
Returns:
bool: True if the robot is initialized for calibration, False otherwise
"""
- return self._mesg(ProtocolCode.IS_INIT_CALIBRATION, has_reply=True)
-
+ return self._mesg(ProtocolCode.IS_INIT_CALIBRATION)
+
def set_break(self, joint_id, value):
"""Set break point
Args:
joint_id: int. joint id 1 - 7
value: int. 0 - disable, 1 - enable
-
+
Return:
0 : failed
1 : success
"""
- self.calibration_parameters(class_name = self.__class__.__name__, id=joint_id, value=value)
- return self._mesg(ProtocolCode.SET_BREAK, joint_id, value, has_reply=True)
-
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id, value=value)
+ return self._mesg(ProtocolCode.SET_BREAK, joint_id, value)
+
def over_limit_return_zero(self):
"""Return to zero when the joint is over the limit
"""
- return self._mesg(ProtocolCode.OVER_LIMIT_RETURN_ZERO)
-
- def jog_increment_angle(self, joint_id, increment, speed):
- """angle step mode
+ return self._mesg(ProtocolCode.OVER_LIMIT_RETURN_ZERO, has_reply=True)
+
+ def jog_increment_angle(self, joint_id, increment, speed, _async=False):
+ """Single angle incremental motion control.
Args:
joint_id: Joint id 1 - 7.
increment:
speed: int (1 - 100)
"""
- self.calibration_parameters(class_name = self.__class__.__name__, id = joint_id, speed = speed)
- return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
-
- def jog_increment_coord(self, coord_id, increment, speed):
- """coord step mode
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id, speed=speed)
+
+ return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed, has_reply=True,
+ _async=_async)
+
+ def jog_increment_coord(self, coord_id, increment, speed, _async=False):
+ """Single coordinate incremental motion control. This interface is based on a single arm 1-axis coordinate system. If you are using a dual arm robot, it is recommended to use the job_base_increment_coord interface
Args:
- coord_id: axis id 1 - 6.
+ joint_id: axis id 1 - 6.
increment:
speed: int (1 - 100)
"""
- self.calibration_parameters(class_name = self.__class__.__name__, id = coord_id, speed = speed)
- value = self._coord2int(increment) if coord_id <= 3 else self._angle2int(increment)
- return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, coord_id, [value], speed)
-
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coord_id=coord_id, speed=speed)
+ value = self._coord2int(
+ increment) if coord_id <= 3 else self._angle2int(increment)
+ return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, coord_id, [value], speed, has_reply=True, _async=_async)
+
+ def get_quick_move_info(self):
+ return self._mesg(ProtocolCode.GET_QUICK_INFO)
+
def drag_teach_clean(self):
"""clear sample
"""
return self._mesg(ProtocolCode.MERCURY_DRAG_TEACH_CLEAN)
-
+
def get_comm_error_counts(self, joint_id, _type):
"""Read the number of communication exceptions
@@ -650,22 +840,19 @@ def get_comm_error_counts(self, joint_id, _type):
3-The number of exceptions sent by the end
4-The number of exceptions read by the end
"""
- return self._mesg(ProtocolCode.MERCURY_ERROR_COUNTS, joint_id, _type, has_reply=True)
-
- def set_pos_over_shoot(self, value):
- """Set position deviation value
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id, _type=_type)
+ return self._mesg(ProtocolCode.MERCURY_ERROR_COUNTS, joint_id, _type)
+
+ def set_pos_over_shoot(self, shoot_value):
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, shoot_value=shoot_value)
+ return self._mesg(ProtocolCode.MERCURY_SET_POS_OVER_SHOOT, [shoot_value * 100])
- Args:
- value (_type_): _description_
- """
- return self._mesg(ProtocolCode.MERCURY_SET_POS_OVER_SHOOT, [value*100])
-
def get_pos_over_shoot(self):
- """Get position deviation value
- """
- return self._mesg(ProtocolCode.MERCURY_GET_POS_OVER_SHOOT, has_reply=True)
-
- def stop(self, deceleration=True):
+ return self._mesg(ProtocolCode.MERCURY_GET_POS_OVER_SHOOT)
+
+ def stop(self, deceleration=0, _async=False):
"""Robot stops moving
Args:
@@ -674,12 +861,15 @@ def stop(self, deceleration=True):
Returns:
int: 1 - Stop completion
"""
- if deceleration:
- return self._mesg(ProtocolCode.STOP, has_reply=True)
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, deceleration=deceleration)
+ # self.is_stop = time.time()
+ if deceleration == 1:
+ return self._mesg(ProtocolCode.STOP, 1, _async=_async)
else:
- return self._mesg(ProtocolCode.STOP, 1, has_reply=True)
-
- def pause(self, deceleration=False):
+ return self._mesg(ProtocolCode.STOP, _async=_async)
+
+ def pause(self, deceleration=0):
"""Robot pauses movement
Args:
@@ -688,23 +878,27 @@ def pause(self, deceleration=False):
Returns:
int: 1 - pause completion
"""
- if deceleration:
- return self._mesg(ProtocolCode.PAUSE, has_reply=True)
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, deceleration=deceleration)
+
+ # self.is_stop = time.time()
+ if deceleration == 1:
+ return self._mesg(ProtocolCode.PAUSE, 1)
else:
- return self._mesg(ProtocolCode.PAUSE, 1, has_reply=True)
-
+ return self._mesg(ProtocolCode.PAUSE)
+
def get_modified_version(self):
- return self._mesg(ProtocolCode.ROBOT_VERSION, has_reply=True)
-
+ return self._mesg(ProtocolCode.ROBOT_VERSION)
+
def get_pos_over(self):
- return self._mesg(ProtocolCode.GET_POS_OVER, has_reply=True)
-
- def clear_encoders_error(self):
- return self._mesg(ProtocolCode.CLEAR_ENCODERS_ERROR)
-
+ return self._mesg(ProtocolCode.GET_POS_OVER)
+
+ def clear_encoders_error(self, joint_id):
+ return self._mesg(ProtocolCode.CLEAR_ENCODERS_ERROR, joint_id)
+
def get_down_encoders(self):
- return self._mesg(ProtocolCode.GET_DOWN_ENCODERS, has_reply=True)
-
+ return self._mesg(ProtocolCode.GET_DOWN_ENCODERS)
+
def set_control_mode(self, mode):
"""Set robot motion mode
@@ -712,16 +906,18 @@ def set_control_mode(self, mode):
mode (int): 0 - location mode, 1 - torque mode
"""
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, mode=mode)
return self._mesg(ProtocolCode.SET_CONTROL_MODE, mode)
-
+
def get_control_mode(self):
"""Get robot motion mode
Returns:
int: 0 - location mode, 1 - torque mode
"""
- return self._mesg(ProtocolCode.GET_CONTROL_MODE, has_reply=True)
-
+ return self._mesg(ProtocolCode.GET_CONTROL_MODE)
+
def set_collision_mode(self, mode):
"""Set collision detection mode
@@ -729,46 +925,52 @@ def set_collision_mode(self, mode):
mode (int): 0 - disable, 1 - enable
"""
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, mode=mode)
return self._mesg(ProtocolCode.SET_COLLISION_MODE, mode)
-
- def set_collision_threshold(self, joint_id, value=100):
+
+ def set_collision_threshold(self, joint_id, threshold_value=100):
"""Set joint collision threshold
Args:
- joint_id (int): joint ID， range 1 ~ 7
- value (int): Collision threshold, range is 50 ~ 250, default is 100, the smaller the value, the easier it is to trigger a collision
+ joint_id (int): joint ID， range 1 ~ 6
+ threshold_value (int): Collision threshold, range is 50 ~ 250, default is 100, the smaller the value, the easier it is to trigger a collision
"""
- return self._mesg(ProtocolCode.SET_COLLISION_THRESHOLD, joint_id, value)
-
+ self.calibration_parameters(class_name=self.__class__.__name__, joint_id=joint_id,
+ threshold_value=threshold_value)
+ return self._mesg(ProtocolCode.SET_COLLISION_THRESHOLD, joint_id, threshold_value)
+
def get_collision_threshold(self):
"""Get joint collision threshold
"""
- return self._mesg(ProtocolCode.GET_COLLISION_THRESHOLD, has_reply=True)
-
- def set_torque_comp(self, joint_id, value=100):
+ return self._mesg(ProtocolCode.GET_COLLISION_THRESHOLD)
+
+ def set_torque_comp(self, joint_id, comp_value=100):
"""Set joint torque compensation
Args:
- joint_id (int): joint ID， range 1 ~ 7
- value (int): Compensation value, range is 0 ~ 250, default is 100, The smaller the value, the harder it is to drag the joint
+ joint_id (int): joint ID， range 1 ~ 6
+ comp_value (int): Compensation value, range is 0 ~ 250, default is 100, The smaller the value, the harder it is to drag the joint
"""
- return self._mesg(ProtocolCode.SET_TORQUE_COMP, joint_id, value)
-
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id, comp_value=comp_value)
+ return self._mesg(ProtocolCode.SET_TORQUE_COMP, joint_id, comp_value)
+
def get_torque_comp(self):
"""Get joint torque compensation
"""
- return self._mesg(ProtocolCode.GET_TORQUE_COMP, has_reply=True)
-
+ return self._mesg(ProtocolCode.GET_TORQUE_COMP)
+
def power_on_only(self):
"""Only turn on the power
"""
- return self._mesg(ProtocolCode.POWER_ON_ONLY, has_reply=True)
-
+ return self._mesg(ProtocolCode.POWER_ON_ONLY)
+
def get_vr_mode(self):
"""Check if the robot is in VR mode
"""
- return self._mesg(ProtocolCode.GET_VR_MODE, has_reply=True)
-
+ return self._mesg(ProtocolCode.GET_VR_MODE)
+
def set_vr_mode(self, mode):
"""Set VR mode
@@ -776,24 +978,24 @@ def set_vr_mode(self, mode):
mode (int): 0 - open, 1 - close
"""
return self._mesg(ProtocolCode.SET_VR_MODE, mode)
-
+
def get_model_direction(self):
"""Get the direction of the robot model
"""
- return self._mesg(ProtocolCode.GET_MODEL_DIRECTION, has_reply=True)
-
- def set_model_direction(self, id, direction):
+ return self._mesg(ProtocolCode.GET_MODEL_DIRECTION)
+
+ def set_model_direction(self, joint_id, direction):
"""Set the direction of the robot model
Args:
- id (int): joint ID, 1 ~ 7.
+ joint_id (int): joint ID, 1 ~ 7.
direction (int): 0 - forward, 1 - backward
"""
- return self._mesg(ProtocolCode.SET_MODEL_DIRECTION, id, direction)
-
+ return self._mesg(ProtocolCode.SET_MODEL_DIRECTION, joint_id, direction)
+
def get_filter_len(self, rank):
"""Get the filter length
-
+
Args:
rank (int):
1 : Drag teaching sampling filter
@@ -802,8 +1004,8 @@ def get_filter_len(self, rank):
4 : Coordinate velocity fusion filter
5 : Drag teaching sampling period
"""
- return self._mesg(ProtocolCode.GET_FILTER_LEN, rank, has_reply=True)
-
+ return self._mesg(ProtocolCode.GET_FILTER_LEN, rank)
+
def set_filter_len(self, rank, value):
"""Set the filter length
@@ -816,4 +1018,761 @@ def set_filter_len(self, rank, value):
5 : Drag teaching sampling period
value (int): Filter length, range is 1 ~ 100
"""
- return self._mesg(ProtocolCode.SET_FILTER_LEN, rank, value)
\ No newline at end of file
+ return self._mesg(ProtocolCode.SET_FILTER_LEN, rank, value)
+
+ def clear_zero_pos(self):
+ return self._mesg(ProtocolCode.CLEAR_ZERO_POS)
+
+ def clear_error_information(self):
+ """Clear robot error message"""
+ return self._mesg(ProtocolCode.CLEAR_ERROR_INFO)
+
+ def get_error_information(self):
+ """Obtaining robot error information
+
+ Return:
+ 0: No error message.
+ 1 ~ 6: The corresponding joint exceeds the limit position.
+ 16 ~ 19: Collision protection.
+ 32: Kinematics inverse solution has no solution.
+ 33 ~ 34: Linear motion has no adjacent solution.
+ """
+ return self._mesg(ProtocolCode.GET_ERROR_INFO)
+
+ def send_angles(self, angles, speed, _async=False):
+ """Send the angles of all joints to robot arm.
+
+ Args:
+ angles: a list of angle values(List[float]). len 7.
+ speed : (int) 1 ~ 100
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, angles=angles, speed=speed)
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.SEND_ANGLES, angles, speed, has_reply=True, _async=_async)
+
+ def send_angle(self, joint_id, angle, speed, _async=False):
+ """Send one angle of joint to robot arm.
+
+ Args:
+ joint_id : Joint id(genre.Angle)， int 1-7.
+ angle : angle value(float).
+ speed : (int) 1 ~ 100
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id, angle=angle, speed=speed)
+ return self._mesg(ProtocolCode.SEND_ANGLE, joint_id, [self._angle2int(angle)], speed, has_reply=True,
+ _async=_async)
+
+ def send_coord(self, coord_id, coord, speed, _async=False):
+ """Send one coord to robot arm.
+
+ Args:
+ coord_id (int): coord id, range 1 ~ 6
+ coord (float): coord value.
+ The coord range of `X` is -351.11 ~ 566.92.
+ The coord range of `Y` is -645.91 ~ 272.12.
+ The coord range of `Y` is -262.91 ~ 655.13.
+ The coord range of `RX` is -180 ~ 180.
+ The coord range of `RY` is -180 ~ 180.
+ The coord range of `RZ` is -180 ~ 180.
+ speed (int): 1 ~ 100
+ """
+
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coord_id=coord_id, coord=coord, speed=speed)
+ value = self._coord2int(
+ coord) if coord_id <= 3 else self._angle2int(coord)
+ return self._mesg(ProtocolCode.SEND_COORD, coord_id, [value], speed, has_reply=True, _async=_async)
+
+ def send_coords(self, coords, speed, _async=False):
+ """Send all coords to robot arm.
+
+ Args:
+ coords: a list of coords value(List[float]). len 6 [x, y, z, rx, ry, rz]
+ The coord range of `X` is -351.11 ~ 566.92.
+ The coord range of `Y` is -645.91 ~ 272.12.
+ The coord range of `Y` is -262.91 ~ 655.13.
+ The coord range of `RX` is -180 ~ 180.
+ The coord range of `RY` is -180 ~ 180.
+ The coord range of `RZ` is -180 ~ 180.
+ speed : (int) 1 ~ 100
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coords=coords, speed=speed)
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(coords[idx]))
+ for angle in coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SEND_COORDS, coord_list, speed, has_reply=True, _async=_async)
+
+ def resume(self):
+ """Recovery movement"""
+ return self._mesg(ProtocolCode.RESUME)
+
+ def set_servo_calibration(self, joint_id):
+ """The current position of the calibration joint actuator is the angle zero point,
+
+ Args:
+ joint_id: Serial number of articulated steering gear. Joint id 1 - 6
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_SERVO_CALIBRATION, joint_id)
+
+ def set_servos_calibration(self):
+ for id in range(1, 8):
+ self._mesg(ProtocolCode.SET_SERVO_CALIBRATION, id)
+
+ def is_in_position(self, data, mode=0):
+ """Judge whether in the position.
+
+ Args:
+ data: A data list, angles or coords. angles len 6, coords len 6.
+ mode: 1 - coords, 0 - angles
+
+ Return:
+ 1 - True\n
+ 0 - False\n
+ -1 - Error
+ """
+ if mode == 1:
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coords=data)
+ data_list = []
+ for idx in range(3):
+ data_list.append(self._coord2int(data[idx]))
+ for idx in range(3, 6):
+ data_list.append(self._angle2int(data[idx]))
+ elif mode == 0:
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, angles=data)
+ data_list = [self._angle2int(i) for i in data]
+ else:
+ raise Exception("mode is not right, please input 0 or 1")
+
+ return self._mesg(ProtocolCode.IS_IN_POSITION, data_list, mode)
+
+ def is_moving(self):
+ """Detect if the robot is moving
+
+ Return:
+ 0 - not moving
+ 1 - is moving
+ -1 - error data
+ """
+ return self._mesg(ProtocolCode.IS_MOVING)
+
+ def jog_angle(self, joint_id, direction, speed, _async=True):
+ """Jog control angle.
+
+ Args:
+ joint_id (int): Joint id 1 - 7.
+ direction (int): 0 - decrease, 1 - increase
+ speed (int): int range 1 - 100
+ _async (bool, optional): Whether to execute asynchronous control. Defaults to True.
+
+ Return:
+ 0: End of exercise.
+ 1 ~ 7: Corresponding joint exceeds limit.
+ 16 ~ 19: Collision protection.
+ 32 ~ 35: Coordinates have no adjacent solutions.
+ 49: Not powered on.
+ 50: Motor abnormality.
+ 51: Motor encoder error
+ 52: Not reaching the designated location or not reaching the designated location for more than 5 minutes (only J11, J12 available)
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id, direction=direction, speed=speed)
+ return self._mesg(ProtocolCode.JOG_ANGLE, joint_id, direction, speed, _async=_async, has_reply=True)
+
+ def jog_coord(self, coord_id, direction, speed, _async=True):
+ """Jog control coord. This interface is based on a single arm 1-axis coordinate system. If you are using a dual arm robot, it is recommended to use the jog_base_coord interface
+
+ Args:
+ coord_id (int): int 1-6
+ direction (int): 0 - decrease, 1 - increase
+ speed (int): 1 - 100
+ _async (bool, optional): Whether to execute asynchronous control. Defaults to True.
+
+ Returns:
+ 1: End of the Movement
+
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coord_id=coord_id, direction=direction, speed=speed)
+ return self._mesg(ProtocolCode.JOG_COORD, coord_id, direction, speed, _async=_async, has_reply=True)
+
+ def get_max_speed(self, mode):
+ """Get maximum speed
+
+ Args:
+ mode (int): 0 - angle speed. 1 - coord speed.
+
+ Return:
+ angle speed range 1 ~ 150°/s. coord speed range 1 ~ 200mm/s
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, mode=mode)
+ return self._mesg(ProtocolCode.GET_SPEED, mode)
+
+ def set_max_speed(self, mode, max_speed):
+ """Set maximum speed
+
+ Args:
+ mode (int): 0 - angle speed. 1 - coord speed.
+ max_speed (int): angle speed range 1 ~ 150°/s. coord speed range 1 ~ 200mm/s
+
+ Returns:
+ 1: _description_
+ """
+ self.calibration_parameters(
+ lass_name=self.__class__.__name__, mode=mode, max_speed=max_speed)
+ return self._mesg(ProtocolCode.SET_SPEED, mode, [max_speed])
+
+ def set_max_acc(self, mode, max_acc):
+ """Set maximum acceleration
+
+ Args:
+ mode (int): 0 - angle acceleration. 1 - coord acceleration.
+ max_acc (int): maximum acceleration value. Angular acceleration range is 1 ~ 150°/s. Coordinate acceleration range is 1 ~ 400mm/s
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, mode=mode, max_acc=max_acc)
+ return self._mesg(ProtocolCode.SET_MAX_ACC, mode, [max_acc])
+
+ def get_max_acc(self, mode):
+ """Get maximum acceleration
+
+ Args:
+ mode (int): 0 - angle acceleration. 1 - coord acceleration.
+ """
+ return self._mesg(ProtocolCode.GET_MAX_ACC, mode)
+
+ def get_joint_min_angle(self, joint_id):
+ """Gets the minimum movement angle of the specified joint
+
+ Args:
+ joint_id: Joint id 1 - 6 or 11 ~ 12
+
+ Return:
+ angle value(float)
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_JOINT_MIN_ANGLE, joint_id)
+
+ def get_joint_max_angle(self, joint_id):
+ """Gets the maximum movement angle of the specified joint
+
+ Args:
+ joint_id: Joint id 1 - 6 or 11 ~ 12
+
+ Return:
+ angle value(float)
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_JOINT_MAX_ANGLE, joint_id)
+
+ def set_joint_max_angle(self, joint_id, degree):
+ """Set the maximum angle of the joint (must not exceed the maximum angle specified for the joint)
+
+ Args:
+ joint_id (int): Joint id 1 - 6 or 11 ~ 12
+ degree: The angle range of joint 1 is -165 ~ 165. The angle range of joint 2 is -55 ~ 95. The angle range of joint 3 is -173 ~ 5. The angle range of joint 4 is -165 ~ 165. The angle range of joint 5 is -20 ~ 265. The angle range of joint 6 is -180 ~ 180. The angle range of joint 11 is -60 ~ 0. The angle range of joint 12 is -138 ~ 188.
+
+ Return:
+ 1 - success
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id, degree=degree)
+ return self._mesg(ProtocolCode.SET_JOINT_MAX, joint_id, degree)
+
+ def set_joint_min_angle(self, joint_id, degree):
+ """Set the minimum angle of the joint (must not be less than the minimum angle specified by the joint)
+
+ Args:
+ joint_id (int): Joint id 1 - 6.
+ degree: The angle range of joint 1 is -165 ~ 165. The angle range of joint 2 is -55 ~ 95. The angle range of joint 3 is -173 ~ 5. The angle range of joint 4 is -165 ~ 165. The angle range of joint 5 is -20 ~ 265. The angle range of joint 6 is -180 ~ 180. The angle range of joint 11 is -60 ~ 0. The angle range of joint 12 is -138 ~ 188.
+
+ Return:
+ 1 - success
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id, degree=degree)
+ return self._mesg(ProtocolCode.SET_JOINT_MIN, joint_id, degree)
+
+ def is_servo_enable(self, joint_id):
+ """To detect the connection state of a single joint
+
+ Args:
+ joint_id: Joint id 1 - 7
+
+ Return:
+ 0 - disable
+ 1 - enable
+ -1 - error
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, joint_id=joint_id)
+ return self._mesg(ProtocolCode.IS_SERVO_ENABLE, joint_id)
+
+ def is_all_servo_enable(self):
+ """Detect the connection status of all joints
+
+ Return:
+ 0 - disable
+ 1 - enable
+ -1 - error
+ """
+ return self._mesg(ProtocolCode.IS_ALL_SERVO_ENABLE)
+
+ def get_servo_data(self, servo_id, address, mode):
+ """Read the data parameter of the specified address of the steering gear.
+
+ Args:
+ servo_id: Joint id 11 or 12
+ address: Data address. range 5 ~ 69
+ mode: 1 - indicates that value is one byte(default), 2 - represents a value of two bytes.
+
+ Return:
+ values 0 - 4096
+ """
+ if mode not in [1, 2]:
+ raise Exception("mode must be 1 or 2")
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, servo_id=servo_id)
+ return self._mesg(
+ ProtocolCode.GET_SERVO_DATA, servo_id, address, mode
+ )
+
+ def set_servo_data(self, servo_id, address, value, mode):
+ """Set the data parameters of the specified address of the steering gear
+
+ Args:
+ servo_id: Joint id 11 or 12
+ address: Data address. range 5 ~ 69
+ value: 0 - 4096
+ mode: 1 - indicates that value is one byte(default), 2 - represents a value of two bytes.
+ """
+ if mode not in [1, 2]:
+ raise Exception("mode must be 1 or 2")
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, servo_id=servo_id)
+ if mode == 1:
+ return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, address, value, mode)
+ else:
+ return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, address, [value], mode)
+
+ def get_servo_speeds(self):
+ """Get joint speed
+
+ Return:
+ unit step/s
+ """
+ return self._mesg(ProtocolCode.GET_SERVO_SPEED)
+
+ def get_servo_currents(self):
+ """Get joint current
+
+ Return:
+ 0 ~ 5000 mA
+ """
+ return self._mesg(ProtocolCode.GET_SERVO_CURRENTS)
+
+ def get_servo_status(self):
+ """Get joint status
+
+ """
+ return self._mesg(ProtocolCode.GET_SERVO_STATUS)
+
+ def set_gripper_state(self, flag, speed, gripper_type=None):
+ """Set gripper switch state
+
+ Args:
+ flag (int): 0 - open, 1 - close
+ speed (int): 1 ~ 100
+ gripper_type (int): default 1
+ 1 : Adaptive gripper. default to adaptive gripper
+ 2 : Parallel Gripper
+ """
+ if gripper_type is None:
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, flag=flag, speed=speed)
+ return self._mesg(ProtocolCode.SET_GRIPPER_STATE, flag, speed)
+ else:
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag, speed=speed,
+ gripper_type=gripper_type)
+ return self._mesg(ProtocolCode.SET_GRIPPER_STATE, flag, speed, gripper_type)
+
+ def set_gripper_value(self, gripper_value, speed, gripper_type=None):
+ """Set gripper value
+
+ Args:
+ gripper_value (int): 0 ~ 100
+ speed (int): 1 ~ 100
+ gripper_type (int): default 1
+ 1 : Adaptive gripper. default to adaptive gripper
+ 2 : Parallel Gripper
+ """
+
+ if gripper_type is not None:
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed,
+ gripper_type=gripper_type)
+ return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, gripper_value, speed, gripper_type)
+ else:
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed)
+ return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, gripper_value, speed)
+
+ def set_gripper_calibration(self):
+ """Set the current position to zero, set current position value is `2048`."""
+ return self._mesg(ProtocolCode.SET_GRIPPER_CALIBRATION)
+
+ def set_gripper_mode(self, mode):
+ """Set gripper mode
+
+ Args:
+ mode: 0 - transparent transmission. 1 - Port Mode.
+
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, mode=mode)
+ return self._mesg(ProtocolCode.SET_GRIPPER_MODE, mode)
+
+ def get_gripper_mode(self):
+ """Get gripper mode
+
+ Return:
+ mode: 0 - transparent transmission. 1 - Port Mode.
+ """
+ return self._mesg(ProtocolCode.GET_GRIPPER_MODE)
+
+ def set_color(self, r=0, g=0, b=0):
+ """Set the light color on the top of the robot end.
+
+ Args:
+ r (int): 0 ~ 255
+ g (int): 0 ~ 255
+ b (int): 0 ~ 255
+
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, rgb=[r, g, b])
+ return self._mesg(ProtocolCode.SET_COLOR, r, g, b)
+
+ def set_digital_output(self, pin_no, pin_signal):
+ """Set the end-of-arm IO status
+
+ Args:
+ pin_no (int): 1 or 2
+ pin_signal (int): 0 / 1
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, pin_no=pin_no, pin_signal=pin_signal)
+ return self._mesg(ProtocolCode.SET_DIGITAL_OUTPUT, pin_no, pin_signal)
+
+ def get_digital_input(self, pin_no):
+ """Read the end-of-arm IO status
+
+ Args:
+ pin_no (int): 1 or 2
+
+ Returns:
+ int: 0 or 1
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, pin_no=pin_no)
+ return self._mesg(ProtocolCode.GET_DIGITAL_INPUT, pin_no)
+
+ def get_digital_inputs(self):
+ """Read the end-of-arm IO status, IN1/IN2/ATOM Button"""
+ return self._mesg(ProtocolCode.GET_DIGITAL_INPUTS)
+
+ def get_world_reference(self):
+ """Get the world coordinate system"""
+ return self._mesg(ProtocolCode.GET_WORLD_REFERENCE)
+
+ def set_world_reference(self, coords):
+ """Set the world coordinate system
+
+ Args:
+ coords: a list of coords value(List[float]). [x(mm), y, z, rx(angle), ry, rz]
+ """
+ # self.calibration_parameters(class_name = self.__class__.__name__, coords=coords)
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(coords[idx]))
+ for angle in coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SET_WORLD_REFERENCE, coord_list)
+
+ def set_tool_reference(self, coords):
+ """Set tool coordinate system
+
+ Args:
+ coords: a list of coords value(List[float])
+ """
+ # self.calibration_parameters(class_name = self.__class__.__name__, coords=coords)
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(coords[idx]))
+ for angle in coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SET_TOOL_REFERENCE, coord_list)
+
+ def get_tool_reference(self):
+ """Get tool coordinate system """
+ return self._mesg(ProtocolCode.GET_TOOL_REFERENCE)
+
+ def set_reference_frame(self, rftype):
+ """Set the base coordinate system
+
+ Args:
+ rftype: 0 - base 1 - tool.
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, rftype=rftype)
+ return self._mesg(ProtocolCode.SET_REFERENCE_FRAME, rftype)
+
+ def get_reference_frame(self):
+ """Get the base coordinate system
+
+ Return:
+ 0 - base 1 - tool.
+ """
+ return self._mesg(ProtocolCode.GET_REFERENCE_FRAME)
+
+ def set_movement_type(self, move_type):
+ """Set movement type
+
+ Args:
+ move_type: 0 - moveJ, 1 - moveL, 2 - moveF, 3 - moveS, 4 - CP
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, move_type=move_type)
+ return self._mesg(ProtocolCode.SET_MOVEMENT_TYPE, move_type)
+
+ def get_movement_type(self):
+ """Get movement type
+
+ Return:
+ 1 - movel, 0 - moveJ
+ """
+ return self._mesg(ProtocolCode.GET_MOVEMENT_TYPE)
+
+ def set_end_type(self, end):
+ """Set end coordinate system
+
+ Args:
+ end: int
+ 0 - flange, 1 - tool
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, end=end)
+ return self._mesg(ProtocolCode.SET_END_TYPE, end)
+
+ def get_end_type(self):
+ """Get end coordinate system
+
+ Return:
+ 0 - flange, 1 - tool
+ """
+ return self._mesg(ProtocolCode.GET_END_TYPE)
+
+ def get_monitor_mode(self):
+ return self._mesg(ProtocolCode.GET_MONITOR_MODE)
+
+ def set_monitor_mode(self, mode):
+ return self._mesg(ProtocolCode.SET_MONITOR_MODE, mode)
+
+ def set_limit_switch(self, limit_mode, state):
+ """Set the limit switches. No save after power off
+
+ Args:
+ limit_mode (int): 1 - Location out of tolerance. 2 - Synchronous control
+ state (int): 0 - close. 1 - open
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, limit_mode=limit_mode, state=state)
+ if limit_mode == 2 and state == 0:
+ self.sync_mode = False
+ elif limit_mode == 2 and state == 1:
+ self.sync_mode = True
+ return self._mesg(ProtocolCode.SET_LIMIT_SWITCH, limit_mode, state)
+
+ def get_limit_switch(self):
+ """Get the limit switches
+
+ Return:
+ list : [Location out of tolerance state, sync state], 0 - close. 1 - open
+ """
+ res = self._mesg(ProtocolCode.GET_LIMIT_SWITCH)
+ if isinstance(res, list) and res[1] == 0:
+ self.sync_mode = False
+ return res
+
+ def solve_inv_kinematics(self, new_coords, old_angles):
+ """_summary_
+ """
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(new_coords[idx]))
+ for angle in new_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ angles = [self._angle2int(angle) for angle in old_angles]
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
+ def get_drag_fifo(self):
+ return self._mesg(ProtocolCode.GET_DRAG_FIFO)
+
+ def set_drag_fifo(self, angles):
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.SET_DRAG_FIFO, angles)
+
+ def get_drag_fifo_len(self):
+ return self._mesg(ProtocolCode.GET_DRAG_FIFO_LEN)
+
+ def jog_rpy(self, axis, direction, speed, _async=True):
+ """Rotate the end point around the fixed axis of the base coordinate system
+
+ Args:
+ axis (int): 1 ~ 3. 1 - Roll, 2 - Pitch, 3 - Yaw
+ direction (int): 1 - Forward. 0 - Reverse.
+ speed (int): 1 ~ 100.
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, axis=axis, direction=direction, speed=speed)
+ return self._mesg(ProtocolCode.JOG_RPY, axis, direction, speed, _async=_async, has_reply=True)
+
+ def get_collision_mode(self):
+ """Get collision detection status
+
+ Return:
+ 1 - open
+ 0 - close
+ """
+ return self._mesg(ProtocolCode.GET_COLLISION_MODE)
+
+ def set_electric_gripper(self, mode):
+ """Set the state of the electric gripper
+
+ Args:
+ mode (int): 0 - open. 1 - close
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, mode=mode)
+ return self._mesg(ProtocolCode.SET_ELECTRIC_GRIPPER, mode)
+
+ def init_electric_gripper(self):
+ """Electric Gripper Initialization
+ """
+ return self._mesg(ProtocolCode.INIT_ELECTRIC_GRIPPER)
+
+ def get_servo_encoder(self, id):
+ return self._mesg(ProtocolCode.GET_ENCODER, id)
+
+ def get_servo_encoders(self):
+ return self._mesg(ProtocolCode.GET_ENCODERS)
+
+ def set_base_io_output(self, pin_no, pin_signal):
+ """Set the base output IO status
+
+ Args:
+ pin_no: pin port number. range 1 ~ 6
+ pin_signal: 0 - low. 1 - high.
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, pin_no=pin_no, pin_signal=pin_signal)
+ return self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, pin_signal)
+
+ def get_base_io_input(self, pin_no):
+ """Get the input IO status of the base
+
+ Args:
+ pin_no: (int) pin port number. range 1 ~ 6
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, pin_no=pin_no)
+ return self._mesg(ProtocolCode.GET_BASIC_INPUT, pin_no)
+
+ def set_world_reference(self, coords):
+ """Set the world coordinate system
+
+ Args:
+ coords: a list of coords value(List[float])
+ for mycobot / mecharm / myArm: [x(mm), y, z, rx(angle), ry, rz]\n
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coords=coords)
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(coords[idx]))
+ for angle in coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SET_WORLD_REFERENCE, coord_list)
+
+ def set_identify_mode(self, mode):
+ return self._mesg(ProtocolCode.SET_IDENTIFY_MODE, mode)
+
+ def get_identify_mode(self):
+ return self._mesg(ProtocolCode.GET_IDENTIFY_MODE)
+
+ def write_move_c_r(self, coords, r, speed, rank=0):
+ """_summary_
+
+ Args:
+ coords (_type_): _description_
+ r (_type_): _description_
+ speed (_type_): _description_
+ rank (_type_): _description_
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coords=coords, r=r, speed=speed, rank=rank)
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(coords[idx]))
+ for angle in coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.WRITE_MOVE_C_R, coord_list, [r * 100], speed, rank, has_reply=True)
+
+ def fourier_trajectories(self, trajectory):
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, trajectory=trajectory)
+ return self._mesg(ProtocolCode.FOURIER_TRAJECTORIES, trajectory)
+
+ def get_dynamic_parameters(self, add):
+ return self._mesg(ProtocolCode.GET_DYNAMIC_PARAMETERS, add)
+
+ def set_dynamic_parameters(self, add, data):
+ return self._mesg(ProtocolCode.SET_DYNAMIC_PARAMETERS, add, [data * 1000])
+
+ def identify_print(self):
+ res = self.all_debug_data
+ self.all_debug_data = []
+ return res
+
+ def clear_encoder_error(self, joint_id):
+ return self._mesg(ProtocolCode.CLEAR_ENCODER_ERROR, joint_id)
+
+ def get_motors_run_err(self):
+ return self._mesg(ProtocolCode.GET_MOTORS_RUN_ERR)
+
+ def get_fusion_parameters(self, rank_mode):
+ """
+ rank_mode: 1 ~ 4
+ """
+ return self._mesg(ProtocolCode.GET_FUSION_PARAMETERS, rank_mode)
+
+ def set_fusion_parameters(self, rank_mode, value):
+ """
+ rank_mode: 1 ~ 4
+ value: 0 ~ 10000
+ """
+ return self._mesg(ProtocolCode.SET_FUSION_PARAMETERS, rank_mode, [value])
+
+ def get_system_version(self):
+ """get system version"""
+ return self._mesg(ProtocolCode.SOFTWARE_VERSION)
diff --git a/pymycobot/common.py b/pymycobot/common.py
index 28c6232..31f6c22 100644
--- a/pymycobot/common.py
+++ b/pymycobot/common.py
@@ -3,11 +3,17 @@
from __future__ import division
import time
import struct
+import logging
import sys
import platform
import locale
+
+import serial
from pymycobot.robot_info import Robot320Info
+from pymycobot.log import setup_logging
+from pymycobot.error import calibration_parameters
+
class ProGripper(object):
SET_GRIPPER_ANGLE = 11
@@ -17,11 +23,80 @@ class ProGripper(object):
SET_GRIPPER_TORQUE = 27
GET_GRIPPER_TORQUE = 28
SET_GRIPPER_SPEED = 32
- GET_GRIPPER_DEFAULT_SPEED = 33
+ GET_GRIPPER_SPEED = 33
SET_GRIPPER_ABS_ANGLE = 36
SET_GRIPPER_PAUSE = 37
SET_GRIPPER_RESUME = 38
SET_GRIPPER_STOP = 39
+ SET_GRIPPER_ANGLES = 45
+
+
+class MyHandGripper(object):
+ GET_HAND_MAJOR_FIRMWARE_VERSION = 1
+ GET_HAND_MINOR_FIRMWARE_VERSION = 2
+ SET_HAND_GRIPPER_ID = 3
+ GET_HAND_GRIPPER_ID = 4
+ SET_HAND_GRIPPER_ENABLED = 0x0A
+ SET_HAND_GRIPPER_ANGLE = 0x0B
+ GET_HAND_GRIPPER_ANGLE = 0x0C
+ SET_HAND_GRIPPER_CALIBRATION = 0x0D
+ GET_HAND_GRIPPER_STATUS = 0x0E
+ SET_HAND_GRIPPER_P = 0x0F
+ GET_HAND_GRIPPER_P = 0x10
+ SET_HAND_GRIPPER_D = 0x11
+ GET_HAND_GRIPPER_D = 0x12
+ SET_HAND_GRIPPER_I = 0x13
+ GET_HAND_GRIPPER_I = 0x14
+ SET_HAND_GRIPPER_CLOCKWISE = 0x15
+ GET_HAND_GRIPPER_CLOCKWISE = 0x16
+ SET_HAND_GRIPPER_COUNTERCLOCKWISE = 0x17
+ GET_HAND_GRIPPER_COUNTERCLOCKWISE = 0x18
+ SET_HAND_GRIPPER_MIN_PRESSURE = 0x19
+ GET_HAND_GRIPPER_MIN_PRESSURE = 0x1A
+ SET_HAND_GRIPPER_TORQUE = 0x1B
+ GET_HAND_GRIPPER_TORQUE = 0x1C
+ SET_HAND_GRIPPER_SPEED = 0x20
+ GET_HAND_GRIPPER_DEFAULT_SPEED = 0x21
+ SET_HAND_GRIPPER_ANGLES = 0x2D
+ GET_HAND_SINGLE_PRESSURE_SENSOR = 0x2E
+ GET_HAND_ALL_PRESSURE_SENSOR = 0x2F
+ GET_HAND_ALL_ANGLES = 0x32
+ SET_HAND_GRIPPER_PINCH_ACTION = 0x33
+ SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT = 0x34
+
+
+class FingerGripper(object):
+ GET_HAND_MAJOR_FIRMWARE_VERSION = 1
+ GET_HAND_MINOR_FIRMWARE_VERSION = 2
+ SET_HAND_GRIPPER_ID = 3
+ GET_HAND_GRIPPER_ID = 4
+ SET_HAND_GRIPPER_ENABLED = 0x0A
+ SET_HAND_GRIPPER_ANGLE = 0x0B
+ GET_HAND_GRIPPER_ANGLE = 0x0C
+ SET_HAND_GRIPPER_CALIBRATION = 0x0D
+ GET_HAND_GRIPPER_STATUS = 0x0E
+ SET_HAND_GRIPPER_P = 0x0F
+ GET_HAND_GRIPPER_P = 0x10
+ SET_HAND_GRIPPER_D = 0x11
+ GET_HAND_GRIPPER_D = 0x12
+ SET_HAND_GRIPPER_I = 0x13
+ GET_HAND_GRIPPER_I = 0x14
+ SET_HAND_GRIPPER_CLOCKWISE = 0x15
+ GET_HAND_GRIPPER_CLOCKWISE = 0x16
+ SET_HAND_GRIPPER_COUNTERCLOCKWISE = 0x17
+ GET_HAND_GRIPPER_COUNTERCLOCKWISE = 0x18
+ SET_HAND_GRIPPER_MIN_PRESSURE = 0x19
+ GET_HAND_GRIPPER_MIN_PRESSURE = 0x1A
+ SET_HAND_GRIPPER_TORQUE = 0x1B
+ GET_HAND_GRIPPER_TORQUE = 0x1C
+ SET_HAND_GRIPPER_SPEED = 0x20
+ GET_HAND_GRIPPER_DEFAULT_SPEED = 0x21
+ SET_HAND_GRIPPER_ANGLES = 0x2D
+ GET_HAND_SINGLE_PRESSURE_SENSOR = 0x2E
+ GET_HAND_ALL_PRESSURE_SENSOR = 0x2F
+ GET_HAND_ALL_ANGLES = 0x32
+ SET_HAND_GRIPPER_PINCH_ACTION = 0x33
+ SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT = 0x34
class ProtocolCode(object):
@@ -35,10 +110,18 @@ class ProtocolCode(object):
GET_ROBOT_ID = 0x03
OVER_LIMIT_RETURN_ZERO = 0x04
SET_ROBOT_ID = 0x04
-
+ CLEAR_COMMAND_QUEUE = 0x05
+ CHECK_ASYNC_OR_SYNC = 0x06
GET_ERROR_INFO = 0x07
CLEAR_ERROR_INFO = 0x08
GET_ATOM_VERSION = 0x09
+
+ CLEAR_ZERO_POS = 0x0A
+ SET_SERVO_CW = 0x0B
+ GET_SERVO_CW = 0x0C
+ CLEAR_WAIST_QUEUE = 0x0D
+ SET_LIMIT_SWITCH = 0x0E
+ GET_LIMIT_SWITCH = 0x0F
SET_POS_SWITCH = 0x0B
GET_POS_SWITCH = 0x0C
@@ -46,8 +129,11 @@ class ProtocolCode(object):
SetHTSGripperTorque = 0x35
GetHTSGripperTorque = 0x36
GetGripperProtectCurrent = 0x37
+ JOG_INCREMENT_BASE_COORD = 0x37
InitGripper = 0x38
SetGripperProtectCurrent = 0x39
+ GET_FUSION_PARAMETERS = 0x4e
+ SET_FUSION_PARAMETERS = 0x4f
# Overall status
POWER_ON = 0x10
@@ -62,7 +148,9 @@ class ProtocolCode(object):
SET_FREE_MODE = 0x1A
IS_FREE_MODE = 0x1B
COBOTX_GET_ANGLE = 0x1C
+ IS_CONTROLLER_BUTTON_PRESSED = 0x1C
POWER_ON_ONLY = 0x1D
+ SET_VISION_MODE = 0x1D
SET_CONTROL_MODE = 0x1E
GET_CONTROL_MODE = 0x1F
FOCUS_ALL_SERVOS = 0x18
@@ -86,6 +174,8 @@ class ProtocolCode(object):
GET_COORD = 0x2D
SEND_ANGLES_AUTO = 0x2E
GET_SOLUTION_ANGLES = 0x2E
+ WRITE_ANGLE_TIME = 0x2E
+ WRITE_WAIST_SYNC = 0x2E
SET_SOLUTION_ANGLES = 0x2F
# JOG MODE AND OPERATION
@@ -93,12 +183,15 @@ class ProtocolCode(object):
JOG_ABSOLUTE = 0x31
JOG_COORD = 0x32
JOG_INCREMENT = 0x33
+ JOG_INCREMENT_COORD = 0x34
+
JOG_STOP = 0x34
JOG_INCREMENT_COORD = 0x34
COBOTX_GET_SOLUTION_ANGLES = 0x35
COBOTX_SET_SOLUTION_ANGLES = 0x36
-
+ JOG_BASE_INCREMENT_COORD = 0x37
+
SET_ENCODER = 0x3A
GET_ENCODER = 0x3B
SET_ENCODERS = 0x3C
@@ -109,8 +202,16 @@ class ProtocolCode(object):
GET_SPEED = 0x40
SET_SPEED = 0x41
GET_FEED_OVERRIDE = 0x42
+ GET_DRAG_FIFO = 0x44
+ SET_DRAG_FIFO = 0x45
+ GET_DRAG_FIFO_LEN = 0x46
+ GET_MAX_ACC = 0x42
+ SET_MAX_ACC = 0x43
GET_ACCELERATION = 0x44
+ GET_DRAG_FIFO = 0x44
+ SET_DRAG_FIFO = 0x45
SET_ACCELERATION = 0x45
+ GET_DRAG_FIFO_LEN = 0x46
GET_JOINT_MIN_ANGLE = 0x4A
GET_JOINT_MAX_ANGLE = 0x4B
SET_JOINT_MIN = 0x4C
@@ -133,6 +234,7 @@ class ProtocolCode(object):
SET_PIN_MODE = 0x60
SET_DIGITAL_OUTPUT = 0x61
GET_DIGITAL_INPUT = 0x62
+ GET_DIGITAL_INPUTS = 0X5F
SET_PWM_MODE = 0x63
SET_PWM_OUTPUT = 0x64
GET_GRIPPER_VALUE = 0x65
@@ -145,13 +247,14 @@ class ProtocolCode(object):
SET_GRIPPER_TORQUE = 0x6F
IS_BTN_CLICKED = 0x6F
SET_COLOR_MYARM = 0x70
- SET_ELETRIC_GRIPPER = 0x6B
- INIT_ELETRIC_GRIPPER = 0x6C
+ SET_ELECTRIC_GRIPPER = 0x6B
+ INIT_ELECTRIC_GRIPPER = 0x6C
SET_GRIPPER_MODE = 0x6D
GET_GRIPPER_MODE = 0x6E
GET_ACCEI_DATA = 0x73
SET_COLLISION_MODE = 0x74
+ GET_COLLISION_MODE = 0xFD
SET_COLLISION_THRESHOLD = 0x75
GET_COLLISION_THRESHOLD = 0x76
SET_TORQUE_COMP = 0x77
@@ -212,14 +315,23 @@ class ProtocolCode(object):
GET_END_TYPE = 0x8A
WRITE_MOVE_C = 0x8C
SOLVE_INV_KINEMATICS = 0x8D
+ SET_IDENTIFY_MODE = 0x8E
+ GET_IDENTIFY_MODE = 0x8F
+ FOURIER_TRAJECTORIES = 0xF8
+ GET_DYNAMIC_PARAMETERS = 0x98
+ SET_DYNAMIC_PARAMETERS = 0x97
+ SOLVE_INV_KINEMATICS = 0x8D
# Impact checking
SET_JOINT_CURRENT = 0x90
GET_JOINT_CURRENT = 0x91
SET_CURRENT_STATE = 0x92
GET_POS_OVER = 0x94
- CLEAR_ENCODERS_ERROR = 0x95
+ CLEAR_ENCODERS_ERROR = 0xEA
GET_DOWN_ENCODERS = 0x96
+ WRITE_MOVE_C_R = 0x99
+
+ GET_MOTORS_RUN_ERR = 0x9C
# planning speed
GET_PLAN_SPEED = 0xD0
@@ -230,6 +342,10 @@ class ProtocolCode(object):
GET_ANGLES_COORDS = 0xD5
GET_QUICK_INFO = 0xD6
SET_FOUR_PIECES_ZERO = 0xD7
+ MERCURY_SET_TOQUE_GRIPPER = 0xD8
+ MERCURY_GET_TOQUE_GRIPPER = 0xD9
+
+ GET_REBOOT_COUNT = 0xD8
# Motor status read
GET_SERVO_SPEED = 0xE1
@@ -244,11 +360,16 @@ class ProtocolCode(object):
GET_TOQUE_GRIPPER = 0xE9
SET_ERROR_DETECT_MODE = 0xE8
GET_ERROR_DETECT_MODE = 0xE9
+ JOG_BASE_RPY = 0xEB
MERCURY_GET_BASE_COORDS = 0xF0
MERCURY_SET_BASE_COORD = 0xF1
MERCURY_SET_BASE_COORDS = 0xF2
MERCURY_JOG_BASE_COORD = 0xF3
+ JOG_RPY = 0xF5
+
+ GET_MONITOR_MODE = 0xFB
+ SET_MONITOR_MODE = 0xFC
MERCURY_DRAG_TECH_SAVE = 0x70
MERCURY_DRAG_TECH_EXECUTE = 0x71
@@ -276,6 +397,7 @@ class ProtocolCode(object):
GET_AUXILIARY_PIN_STATUS = 0xa1
SET_SERVO_MOTOR_CLOCKWISE = 0x73
GET_SERVO_MOTOR_CLOCKWISE = 0Xea
+ CLEAR_ENCODER_ERROR = 0xEA
SET_SERVO_MOTOR_COUNTER_CLOCKWISE = 0x74
GET_SERVO_MOTOR_COUNTER_CLOCKWISE = 0xeb
SET_SERVO_MOTOR_CONFIG = 0x52
@@ -304,6 +426,12 @@ class ProtocolCode(object):
# SET_IIC_STATE = 0xA4
# GET_IIS_BYTE = 0xA5
# SET_IIC_BYTE = 0xA6
+ DRAG_START_RECORD = 0xF1
+ DRAG_END_RECORD = 0xF2
+ DRAG_GET_RECORD_DATA = 0xF3
+ DRAG_GET_RECORD_LEN = 0xF4
+ DRAG_CLEAR_RECORD_DATA = 0xF5
+
# ultraArm
END = "\r"
@@ -335,9 +463,23 @@ class ProtocolCode(object):
GET_GRIPPER_ANGLE = "M50"
SET_SYSTEM_VALUE = "M51"
GET_SYSTEM_VALUE = "M52"
+ GET_SYSTEM_VERSION = "G6"
+ GET_MODIFY_VERSION = "G7"
class DataProcessor(object):
+ crc_robot_class = ["Mercury", "MercurySocket", "Pro630", "Pro630Client", "Pro400Client", "Pro400", "MercuryTest"]
+ def __init__(self, debug=False):
+ """
+ Args:
+ debug : whether show debug info
+ """
+ self._version = sys.version_info[:2][0]
+ self.debug = debug
+ setup_logging(self.debug)
+ self.log = logging.getLogger(__name__)
+ self.calibration_parameters = calibration_parameters
+
def _mesg(self, genre, *args, **kwargs):
"""
Args:
@@ -356,8 +498,7 @@ def _mesg(self, genre, *args, **kwargs):
elif genre in [76, 77]:
command_data = [command_data[0]] + self._encode_int16(command_data[1] * 10)
- elif genre == 115 and self.__class__.__name__ not in ["MyArmC", "MyArmM", "Mercury", "MercurySocket", "Pro630",
- "Pro630Client", "Pro400Client", "Pro400"]:
+ elif genre == 115 and self.__class__.__name__ not in self.crc_robot_class and self.__class__.__name__ not in ['MyArmC', 'MyArmM']:
command_data = [command_data[1], command_data[3]]
LEN = len(command_data) + 2
@@ -369,7 +510,7 @@ def _mesg(self, genre, *args, **kwargs):
]
if command_data:
command.extend(command_data)
- if self.__class__.__name__ in ["Mercury", "MercurySocket", "Pro630", "Pro630Client", "Pro400Client", "Pro400"]:
+ if self.__class__.__name__ in self.crc_robot_class:
command[2] += 1
command.extend(self.crc_check(command))
else:
@@ -377,7 +518,8 @@ def _mesg(self, genre, *args, **kwargs):
real_command = self._flatten(command)
has_reply = kwargs.get("has_reply", False)
- return real_command, has_reply
+ _async = kwargs.get("_async", False)
+ return real_command, has_reply, _async
# Functional approach
def _encode_int8(self, data):
@@ -411,6 +553,9 @@ def _coord2int(self, coord):
def _int2angle(self, _int):
return round(_int / 100.0, 3)
+
+ def _int3angle(self, _int):
+ return round(_int / 1000, 3)
def _int2coord(self, _int):
return round(_int / 10.0, 2)
@@ -462,14 +607,9 @@ def _process_data_command(self, genre, _class, args):
processed_args = []
for index in range(len(args)):
if isinstance(args[index], list):
- if genre in [ProtocolCode.SET_ENCODERS_DRAG] and index in [0, 1] and _class in ["Mercury",
- "MercurySocket",
- "Pro630",
- "Pro630Client",
- "Pro400Client",
- "Pro400"]:
+ if genre in [ProtocolCode.SET_ENCODERS_DRAG, ProtocolCode.SET_DYNAMIC_PARAMETERS] and index in [0, 1] and _class in self.crc_robot_class:
for data in args[index]:
- byte_value = data.to_bytes(4, byteorder='big', signed=True)
+ byte_value = int(data).to_bytes(4, byteorder='big', signed=True)
res = []
for i in range(len(byte_value)):
res.append(byte_value[i])
@@ -480,9 +620,7 @@ def _process_data_command(self, genre, _class, args):
if isinstance(args[index], str):
processed_args.append(args[index])
else:
- if genre == ProtocolCode.SET_SERVO_DATA and _class in ["Mercury", "MercurySocket", "Pro630",
- "Pro630Client", "Pro400Client",
- "Pro400"] and index == 2:
+ if genre == ProtocolCode.SET_SERVO_DATA and _class in self.crc_robot_class and index == 2:
byte_value = args[index].to_bytes(2, byteorder='big', signed=True)
res = []
for i in range(len(byte_value)):
@@ -498,8 +636,8 @@ def _is_frame_header(self, data, pos1, pos2):
def _process_received(self, data, genre, arm=6):
if genre == 177:
- data = str(data)[2:-1].split(": ")
- return data[1][0:-9], data[-1]
+ data = data.decode("utf-8").split(" ")
+ return data[1], data[-1]
elif not data:
return None
elif data == b'\xfe\xfe\x04[\x01\r\x87':
@@ -528,7 +666,7 @@ def _process_received(self, data, genre, arm=6):
data_len = data[header_i + 3] - 2
elif arm == 14:
data_len = data[header_i + 2] - 3
-
+ # unique_data = [ProtocolCode.GET_BASIC_INPUT, ProtocolCode.GET_DIGITAL_INPUT, ProtocolCode.SET_BASIC_OUTPUT]
unique_data = [ProtocolCode.GET_BASIC_INPUT, ProtocolCode.GET_DIGITAL_INPUT]
if cmd_id == ProtocolCode.GET_DIGITAL_INPUT and arm == 14:
data_pos = header_i + 4
@@ -645,9 +783,16 @@ def _process_received(self, data, genre, arm=6):
else:
res.append(i)
elif data_len == 28:
- for i in range(0, data_len, 4):
- byte_value = int.from_bytes(valid_data[i:i + 4], byteorder='big', signed=True)
- res.append(byte_value)
+ if genre == ProtocolCode.GET_QUICK_INFO:
+ for header_i in range(0, len(valid_data)-2, 2):
+ one = valid_data[header_i : header_i + 2]
+ res.append(self._decode_int16(one))
+ res.append(valid_data[-2])
+ res.append(valid_data[-1])
+ else:
+ for i in range(0, data_len, 4):
+ byte_value = int.from_bytes(valid_data[i:i+4], byteorder='big', signed=True)
+ res.append(byte_value)
elif data_len == 40:
i = 0
while i < data_len:
@@ -676,6 +821,21 @@ def byte2int(bvs):
return list(map(lambda _i: self._decode_int16(bvs[_i:_i + 2]), range(0, 16, 2)))
return [byte2int(valid_data[0:16]), byte2int(valid_data[16:32])]
+ elif data_len == 48:
+ if genre == ProtocolCode.GET_QUICK_INFO:
+ i = 0
+ while i < data_len:
+ if i < 28:
+ byte_value = int.from_bytes(valid_data[i:i+4], byteorder='big', signed=True)
+ res.append(byte_value)
+ i+=4
+ elif i < 40:
+ one = valid_data[i : i + 2]
+ res.append(self._decode_int16(one))
+ i+=2
+ else:
+ res.append(valid_data[i])
+ i+=1
elif data_len == 38:
i = 0
res = []
@@ -730,7 +890,6 @@ def check_python_version():
else:
return -1
-
def write(self, command, method=None):
if len(command) > 3 and command[3] == 176 and len(command) > 5:
command = "'" + command[4] + "'" + "(" + command[5] + ")"
@@ -746,27 +905,53 @@ def write(self, command, method=None):
py_version = DataProcessor.check_python_version()
if py_version == 2:
- self.sock.sendall("".join([chr(b) for b in command]))
+ # 检查套接字是否有效
+ if not self.sock or self.sock.fileno() == -1:
+ return
+
+ try:
+ self.sock.sendall("".join([chr(b) for b in command]))
+ except OSError as e:
+ raise e
else:
if isinstance(command, str):
command = command.encode()
else:
command = bytes(command)
- self.sock.sendall(command)
+ if not self.sock or self.sock.fileno() == -1:
+ return
+
+ try:
+ self.sock.sendall(command)
+ except OSError as e:
+ self.log.error(f"Socket send error: {e}")
+ raise
+
else:
- self._serial_port.reset_input_buffer()
+ # self._serial_port.reset_input_buffer()
command_log = ""
for i in command:
if isinstance(i, str):
command_log += i[2:] + " "
else:
- command_log += hex(i)[2:] + " "
+ data = hex(i)[2:]
+ if len(data) == 1: data = "0" + data
+ command_log += data + " "
self.log.debug("_write: {}".format(command_log))
- self._serial_port.write(command)
- self._serial_port.flush()
+ try:
+ # 检查串口是否打开
+ if not self._serial_port.isOpen():
+ return
+
+ self._serial_port.write(command)
+ except serial.SerialException as e:
+ self.log.error(f"Serial port error: {e}")
+ # self._serial_port.write(command)
+ # self._serial_port.flush()
-def read(self, genre, method=None, command=None, _class=None, timeout=None):
+
+def read(self, genre, method=None, command=None, _class=None, timeout=None, real_command=None):
datas = b""
data_len = -1
k = 0
@@ -780,7 +965,7 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
wait_time = 0.3
if timeout is not None:
wait_time = timeout
- if _class in ["Mercury", "MercurySocket", "Pro630", "Pro630Client", "Pro400Client", "Pro400"]:
+ if _class in DataProcessor.crc_robot_class:
if genre == ProtocolCode.POWER_ON:
wait_time = 8
elif genre in [ProtocolCode.POWER_OFF, ProtocolCode.RELEASE_ALL_SERVOS, ProtocolCode.FOCUS_ALL_SERVOS,
@@ -791,12 +976,31 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
ProtocolCode.JOG_INCREMENT, ProtocolCode.JOG_INCREMENT_COORD,
ProtocolCode.COBOTX_SET_SOLUTION_ANGLES]:
wait_time = 300
- elif _class in ["MyCobot", "MyCobotSocket"]:
+ elif _class in ["MyCobot", "MyCobotSocket", "MyCobot320", "MyCobot320Socket"]:
if genre == ProtocolCode.GET_ROBOT_STATUS:
wait_time = 75
+ elif genre == ProtocolCode.GET_ROBOT_STATUS:
+ wait_time = 90
+ elif genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
+ wait_time = 0.05
+ elif genre in [ProtocolCode.INIT_ELECTRIC_GRIPPER, ProtocolCode.SET_GRIPPER_MODE]:
+ wait_time = 0.3
+ elif genre in [ProtocolCode.SET_GRIPPER_CALIBRATION]:
+ wait_time = 0.4
+ if real_command:
+ if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+ if len(real_command) == 12:
+ if real_command[6] == 13:
+ wait_time = 10
+ else:
+ if real_command[6] == 13:
+ wait_time = 3
data = b""
-
if method is not None:
+ if real_command:
+ if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+ if real_command[6] == 13:
+ wait_time = 3
if genre == 177:
while True:
data = self.sock.recv(1024)
@@ -841,7 +1045,7 @@ def read(self, genre, method=None, command=None, _class=None, timeout=None):
data = self._serial_port.read()
# self.log.debug("data: {}".format(data))
k += 1
- if _class in ["Mercury", "MercurySocket", "Pro630", "Pro630Client", "Pro400Client", "Pro400"]:
+ if _class in DataProcessor.crc_robot_class:
if data_len == 3:
datas += data
crc = self._serial_port.read(2)
diff --git a/pymycobot/conveyor_api.py b/pymycobot/conveyor_api.py
new file mode 100644
index 0000000..d55d623
--- /dev/null
+++ b/pymycobot/conveyor_api.py
@@ -0,0 +1,212 @@
+import logging
+import threading
+import time
+import struct
+from serial import Serial
+
+
+class CommandGenre(object):
+ SET_SERVO_DIRECTION = 0XA0
+ GET_SERVO_DIRECTION = 0XA2
+
+ SET_SERVO_SPEED = 0XA5
+ GET_SERVO_SPEED = 0XA3
+
+ # WRITE_SERVO_ANGLE = 0XA6
+ # WRITE_SERVO_STEP = 0XA7
+
+ READ_FIRMWARE_VERSION = 0XAA
+
+
+class Command(object):
+ HEADER = 0XFF
+
+ def __init__(self, genre, address, check_digit, params, length=None):
+ self.__header = [0xff, 0xff]
+ self.__length = length or len(params)
+ self.__address = address
+ self.__params = params
+ self.__genre = genre
+ self.__check_digit = check_digit
+
+ @property
+ def genre(self):
+ return self.__genre
+
+ def to_bytes(self):
+ return bytes([*self.__header, self.__address, self.__length, *self.__params, self.__genre, *self.__check_digit])
+
+ def get_params(self):
+ return self.__params[0] if self.__length == 1 else self.__params
+
+ def __str__(self):
+ return " ".join(map(lambda bit: hex(bit).upper(), self.to_bytes()))
+
+ def __bytes__(self):
+ return self.to_bytes()
+
+ @classmethod
+ def packing(cls, genre: int, addr: int, *params):
+ check_code = cls.check_digit(genre, params)
+ return cls(genre=genre, address=addr, check_digit=(check_code, ), params=(*params,))
+
+ @classmethod
+ def parsing(cls, buffer: bytes):
+ # header, header, addr, length, params, genre, check_bits
+ if len(buffer) < 6:
+ return None
+ if buffer[0] != cls.HEADER or buffer[1] != cls.HEADER:
+ return None
+ length = buffer[3]
+ return cls(genre=buffer[-2], address=buffer[2], length=length, params=(*buffer[4:4+length], ), check_digit=buffer[4+length:4+length+1])
+
+ @classmethod
+ def unpack_args(cls, *parameters):
+ bits_pack_list = []
+ for param in parameters:
+ pair = struct.pack('>h', param)
+ if len(pair) == 2:
+ bits_pack_list.extend(list(pair))
+ else:
+ bits_pack_list.clear()
+ print(bits_pack_list)
+ return bits_pack_list
+
+ @classmethod
+ def check_digit(cls, genre, params):
+ """
+ Calculate the check-code for the command.
+ :param genre: int, function genre
+ :param params: bytes, function parameters
+ :return: int, check-code
+ """
+ return sum([genre, *params]) & 0xff
+
+ @classmethod
+ def has_header(cls, buffer: bytes):
+ """
+ Check if the buffer contains a header.
+ :param buffer:
+ :return:
+ """
+ if len(buffer) < 2:
+ return False
+ return buffer[0] == cls.HEADER and buffer[1] == cls.HEADER
+
+
+class SerialProtocol(object):
+
+ def __init__(self, comport, baudrate, timeout=0.5):
+ self._comport = comport
+ self._baudrate = baudrate
+ self._timeout = timeout
+ self._serial_port = Serial(port=comport, baudrate=baudrate, timeout=timeout)
+ self._serial_port.rts = True
+ self._serial_port.dtr = True
+
+ def open(self):
+ if self._serial_port.is_open is False:
+ self._serial_port.open()
+
+ def close(self):
+ if self._serial_port.is_open is True:
+ self._serial_port.close()
+
+ def write(self, data: bytes):
+ self._serial_port.write(data)
+ self._serial_port.flush()
+
+ def read(self, size=None):
+ return self._serial_port.read(size or self._serial_port.in_waiting)
+
+ def flush(self):
+ self._serial_port.flush()
+
+
+class ConveyorAPI(SerialProtocol):
+ class MotorModel:
+ STEPPER_MOTOR_42 = 0x30
+ STEPPER_MOTOR_57 = 0x31
+
+ def __init__(self, comport, baudrate="115200", timeout=0.1, debug=False):
+ super().__init__(comport, baudrate, timeout)
+ self._debug = debug
+ self.open()
+ self._mutex = threading.Lock()
+ self._log = logging.getLogger("conveyor_api")
+ self._log.setLevel(logging.DEBUG if debug else logging.INFO)
+ handler = logging.StreamHandler()
+ handler.setLevel(logging.DEBUG if debug else logging.INFO)
+ formatter = logging.Formatter(
+ fmt='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
+ datefmt='%Y-%m-%d %H:%M:%S'
+ )
+ handler.setFormatter(formatter)
+ self._log.addHandler(handler)
+
+ def _wait_for_reply(self, timeout=None):
+ buffers = b""
+ timeout = timeout or self._timeout
+ start_time = time.perf_counter()
+ while time.perf_counter() - start_time < timeout:
+ if self._serial_port.in_waiting <= 0:
+ continue
+ buffers = self.read(self._serial_port.in_waiting)
+ if not Command.has_header(buffers):
+ continue
+ break
+
+ return Command.parsing(buffers) if Command.has_header(buffers) else None
+
+ def _merge(self, genre, address, *parameters, has_reply=False):
+ command = Command.packing(genre, address, *parameters)
+ self._log.debug(f"write > {command}")
+ with self._mutex:
+ self.write(command.to_bytes())
+
+ if not has_reply:
+ return
+
+ reply_command = self._wait_for_reply(timeout=0.07) # WaitForAReply
+ self._log.debug(f"read < {reply_command}")
+ if not reply_command:
+ return None
+
+ result = reply_command.get_params()
+ if reply_command.genre == CommandGenre.READ_FIRMWARE_VERSION:
+ return result / 10
+ return result
+
+ def set_motor_direction(self, direction, motor_model=MotorModel.STEPPER_MOTOR_57):
+ """Modify the direction of movement of the conveyor belt"""
+ if direction not in (0, 1):
+ raise ValueError("direction must be 0 or 1")
+ self._merge(CommandGenre.SET_SERVO_DIRECTION, motor_model, direction)
+
+ def get_motor_direction(self, motor_model=MotorModel.STEPPER_MOTOR_57):
+ """Get the direction of movement of the conveyor belt"""
+ return self._merge(CommandGenre.GET_SERVO_DIRECTION, motor_model, has_reply=True)
+
+ def get_motor_speed(self, motor_model=MotorModel.STEPPER_MOTOR_57):
+ """Get the speed of the conveyor belt"""
+ return self._merge(CommandGenre.GET_SERVO_SPEED, motor_model, has_reply=True)
+
+ def set_motor_speed(self, status, speed: int, motor_model=MotorModel.STEPPER_MOTOR_57):
+ """Modify the speed of the conveyor belt"""
+ if status not in (0, 1):
+ raise ValueError("status must be 0 or 1")
+
+ if not 0 <= speed <= 100:
+ raise ValueError("speed must be in range [0, 100]")
+ return self._merge(CommandGenre.SET_SERVO_SPEED, motor_model, status, speed)
+
+ # def write_motor_angle(self, carry, angle: int, speed, motor_model=MotorModel.STEPPER_MOTOR_57):
+ # return self._merge(CommandGenre.WRITE_SERVO_ANGLE, motor_model, carry, angle, speed)
+ #
+ # def write_motor_step(self, carry, step: int, speed, direction, motor_model=MotorModel.STEPPER_MOTOR_57):
+ # return self._merge(CommandGenre.WRITE_SERVO_STEP, motor_model, carry, step, speed, direction)
+
+ def read_firmware_version(self, motor_model=MotorModel.STEPPER_MOTOR_57):
+ """Get the firmware version of the conveyor belt"""
+ return self._merge(CommandGenre.READ_FIRMWARE_VERSION, motor_model, has_reply=True)
+
diff --git a/pymycobot/dualcobotx.py b/pymycobot/dualcobotx.py
new file mode 100644
index 0000000..812aad1
--- /dev/null
+++ b/pymycobot/dualcobotx.py
@@ -0,0 +1,267 @@
+# coding=utf-8
+
+import time
+import threading
+
+from pymycobot.generate import CommandGenerator
+from pymycobot.common import ProtocolCode, write, read
+from pymycobot.error import calibration_parameters
+
+
+class DualMercury(CommandGenerator):
+ _write = write
+ _read = read
+ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
+ """
+ Args:
+ port : port string
+ baudrate : baud rate string, default '115200'
+ timeout : default 0.1
+ debug : whether show debug info
+ """
+ super(DualMercury, self).__init__(debug)
+ self.calibration_parameters = calibration_parameters
+ import serial
+
+ self._serial_port = serial.Serial()
+ self._serial_port.port = port
+ self._serial_port.baudrate = baudrate
+ self._serial_port.timeout = timeout
+ self._serial_port.rts = False
+ self._serial_port.open()
+ self.lock = threading.Lock()
+
+
+ def _mesg(self, genre, *args, **kwargs):
+ """
+
+ Args:
+ genre: command type (Command)
+ *args: other data.
+ It is converted to octal by default.
+ If the data needs to be encapsulated into hexadecimal,
+ the array is used to include them. (Data cannot be nested)
+ **kwargs: support `has_reply`
+ has_reply: Whether there is a return value to accept.
+ """
+ real_command, has_reply = super(Mercury, self)._mesg(genre, *args, **kwargs)
+ with self.lock:
+ self._write(self._flatten(real_command))
+
+ if has_reply:
+ data = self._read(genre)
+ if genre == ProtocolCode.SET_SSID_PWD:
+ return None
+ res = self._process_received(data, genre)
+ if genre in [
+ ProtocolCode.ROBOT_VERSION,
+ ProtocolCode.GET_ROBOT_ID,
+ ProtocolCode.IS_POWER_ON,
+ ProtocolCode.IS_CONTROLLER_CONNECTED,
+ ProtocolCode.IS_PAUSED, # TODO have bug: return b''
+ ProtocolCode.IS_IN_POSITION,
+ ProtocolCode.IS_MOVING,
+ ProtocolCode.IS_SERVO_ENABLE,
+ ProtocolCode.IS_ALL_SERVO_ENABLE,
+ ProtocolCode.GET_SERVO_DATA,
+ ProtocolCode.GET_DIGITAL_INPUT,
+ ProtocolCode.GET_GRIPPER_VALUE,
+ ProtocolCode.IS_GRIPPER_MOVING,
+ ProtocolCode.GET_SPEED,
+ ProtocolCode.GET_ENCODER,
+ ProtocolCode.GET_BASIC_INPUT,
+ ProtocolCode.GET_TOF_DISTANCE,
+ ProtocolCode.GET_END_TYPE,
+ ProtocolCode.GET_MOVEMENT_TYPE,
+ ProtocolCode.GET_REFERENCE_FRAME,
+ ProtocolCode.GET_FRESH_MODE,
+ ProtocolCode.GET_GRIPPER_MODE,
+ ProtocolCode.SET_SSID_PWD,
+ ProtocolCode.COBOTX_IS_GO_ZERO,
+ ]:
+ return self._process_single(res)
+ elif genre in [ProtocolCode.GET_ANGLES]:
+ return [self._int2angle(angle) for angle in res]
+ elif genre in [
+ ProtocolCode.GET_COORDS,
+ ProtocolCode.GET_TOOL_REFERENCE,
+ ProtocolCode.GET_WORLD_REFERENCE,
+ ]:
+ if res:
+ r = []
+ for idx in range(3):
+ r.append(self._int2coord(res[idx]))
+ for idx in range(3, 6):
+ r.append(self._int2angle(res[idx]))
+ return r
+ else:
+ return res
+ elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
+ return [self._int2coord(angle) for angle in res]
+ elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION]:
+ return self._int2coord(self._process_single(res))
+ elif genre in [
+ ProtocolCode.GET_JOINT_MAX_ANGLE,
+ ProtocolCode.GET_JOINT_MIN_ANGLE,
+ ]:
+ return self._int2coord(res[0])
+ elif genre == ProtocolCode.GET_ANGLES_COORDS:
+ r = []
+ for index in range(len(res)):
+ if index < 7:
+ r.append(self._int2angle(res[index]))
+ elif index < 10:
+ r.append(self._int2coord(res[index]))
+ else:
+ r.append(self._int2angle(res[index]))
+ return r
+ elif genre == ProtocolCode.GO_ZERO:
+ r = []
+ if 1 not in res[1:]:
+ return res[0]
+ else:
+ for i in range(1, len(res)):
+ if res[i] == 1:
+ r.append(i)
+ return r
+ elif genre in [ProtocolCode.COBOTX_GET_ANGLE, ProtocolCode.GET_SOLUTION_ANGLES]:
+ return self._int2angle(res[0])
+ else:
+ return res
+ return None
+
+ def set_solution_angles(self, angle, speed):
+ """Set zero space deflection angle value
+
+ Args:
+ angle: Angle of joint 1. The angle range is -90 ~ 90
+ speed: 1 - 100.
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, speed=speed, solution_angle=angle
+ )
+ return self._mesg(
+ ProtocolCode.COBOTX_SET_SOLUTION_ANGLES, [self._angle2int(angle)], speed
+ )
+
+ def get_solution_angles(self):
+ """Get zero space deflection angle value"""
+ return self._mesg(ProtocolCode.COBOTX_GET_SOLUTION_ANGLES, has_reply=True)
+
+ def write_move_c(self, transpoint, endpoint, speed):
+ """_summary_
+
+ Args:
+ transpoint (list): Arc passing point coordinates
+ endpoint (list): Arc end point coordinates
+ speed (int): 1 ~ 100
+ """
+ start = []
+ end = []
+ for index in range(6):
+ if index < 3:
+ start.append(self._coord2int(transpoint[index]))
+ end.append(self._coord2int(endpoint[index]))
+ else:
+ start.append(self._angle2int(transpoint[index]))
+ end.append(self._angle2int(endpoint[index]))
+ return self._mesg(ProtocolCode.WRITE_MOVE_C, start, end, speed)
+
+ def focus_all_servos(self):
+ """Lock all joints"""
+ return self._mesg(ProtocolCode.FOCUS_ALL_SERVOS)
+
+ def go_zero(self):
+ """Control the machine to return to the zero position.
+
+ Return:
+ 1 : All motors return to zero position.
+ list : Motor with corresponding ID failed to return to zero.
+ """
+ return self._mesg(ProtocolCode.GO_ZERO, has_reply=True)
+
+ def get_angle(self, joint_id):
+ """Get single joint angle
+
+ Args:
+ joint_id (int): 1 ~ 7 or 11 ~ 13.
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id)
+ return self._mesg(ProtocolCode.COBOTX_GET_ANGLE, joint_id, has_reply=True)
+
+ def is_gone_zero(self):
+ """Check if it has returned to zero
+
+ Return:
+ 1 : Return to zero successfully.
+ 0 : Returning to zero.
+ """
+ return self._mesg(ProtocolCode.COBOTX_IS_GO_ZERO, has_reply=True)
+
+ def set_encoder(self, joint_id, encoder):
+ """Set a single joint rotation to the specified potential value.
+
+ Args:
+ joint_id (int): Joint id 1 - 7.
+ encoder: The value of the set encoder.
+ """
+ # TODO Mercury此接口待定
+ # self.calibration_parameters(
+ # class_name=self.__class__.__name__, id=joint_id, encoder=encoder
+ # )
+ return self._mesg(ProtocolCode.SET_ENCODER, joint_id, [encoder])
+
+
+ def servo_restore(self, joint_id):
+ """Abnormal recovery of joints
+
+ Args:
+ joint_id (int): Joint ID.
+ arm : 1 ~ 7
+ waist : 13
+ All joints: 254
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, servo_restore=joint_id
+ )
+ self._mesg(ProtocolCode.SERVO_RESTORE, joint_id)
+
+ def close(self):
+ self._serial_port.close()
+
+ def open(self):
+ self._serial_port.open()
+
+ def sync_send_angles(self, degrees, speed, timeout=15):
+ """Send the angle in synchronous state and return when the target point is reached
+
+ Args:
+ degrees: a list of degree values(List[float]), length 6.
+ speed: (int) 0 ~ 100
+ timeout: default 7s.
+ """
+ t = time.time()
+ self.send_angles(degrees, speed)
+ while time.time() - t < timeout:
+ f = self.is_in_position(degrees, 0)
+ if f == 1:
+ break
+ time.sleep(0.1)
+ return self
+
+ def sync_send_coords(self, coords, speed, mode=None, timeout=15):
+ """Send the coord in synchronous state and return when the target point is reached
+
+ Args:
+ coords: a list of coord values(List[float])
+ speed: (int) 0 ~ 100
+ mode: (int): 0 - angular（default）, 1 - linear
+ timeout: default 7s.
+ """
+ t = time.time()
+ self.send_coords(coords, speed, mode)
+ while time.time() - t < timeout:
+ if self.is_in_position(coords, 1) == 1:
+ break
+ time.sleep(0.1)
+ return self
diff --git a/pymycobot/elephantrobot.py b/pymycobot/elephantrobot.py
index 3ae59ba..ce244d4 100644
--- a/pymycobot/elephantrobot.py
+++ b/pymycobot/elephantrobot.py
@@ -2,11 +2,41 @@
from socket import socket, AF_INET, SOCK_STREAM
import sys
+from enum import Enum
import base64
import hashlib
+import math
+import numpy as np
from multiprocessing import Lock
import logging
from pymycobot.log import setup_logging
+from pymycobot.tool_coords import *
+
+
+COORDS_EPSILON = 0.50
+
+
+class Axis(Enum):
+ X = 0
+ Y = 1
+ Z = 2
+ RX = 3
+ RY = 4
+ RZ = 5
+
+
+class Joint(Enum):
+ J1 = 0
+ J2 = 1
+ J3 = 2
+ J4 = 3
+ J5 = 4
+ J6 = 5
+
+
+class JogMode(Enum):
+ JOG_JOINT = 0
+ JOG_TELEOP = 1
mutex = Lock()
@@ -21,10 +51,42 @@ def __init__(self, host, port, debug=False):
self.BUFFSIZE = 8 * 1024 * 1024
self.ADDR = (host, port)
self.tcp_client = socket(AF_INET, SOCK_STREAM)
+ self.is_client_started = False
+ tool_reference = np.array([0, 0, 0, 0, 0, 0])
+ self.tool_matrix = self.set_tool_reference(tool_reference)
+
+ def get_ip(self):
+ """Returns tuple of (IP address or hostname, port) of robot with socket server.
+
+ Returns:
+ str: IP address or hostname
+ """
+ return self.ADDR
+
+ def set_ip(self, host, port):
+ """Sets IP address or hostname and port of robot with socket server.
+ Cannot set or change IP address or hostname or port of already started client.
+ Need to stop client before setting IP address/hostname/port.
+
+ Args:
+ host (str): IP address or hostname
+ port (int): port
+
+ Returns:
+ bool: True if success, False otherwise
+ """
+ if self.is_client_started:
+ print(
+ "Error: Cannot change IP if client is started. Stop client to change IP/Host/Port"
+ )
+ return False
+ self.ADDR = (host, port)
+ return True
def start_client(self):
try:
self.tcp_client.connect(self.ADDR)
+ self.is_client_started = True
return True
except Exception as e:
print(e)
@@ -32,6 +94,7 @@ def start_client(self):
def stop_client(self):
self.tcp_client.close()
+ self.is_client_started = False
def send_command(self, command):
with mutex:
@@ -71,6 +134,58 @@ def string_to_double(self, data):
except:
return -9999.99
+ def float_equal(self, a, b, epsilon=COORDS_EPSILON):
+ """Compares 2 floats with given epsilon precision.
+
+ Args:
+ a (float): 1st float to compare
+ b (float): 2nd float to compare
+ epsilon (float): precision (epsilon) to compare
+
+ Returns:
+ bool: True if floats are considered equal with given precision,
+ False otherwise
+ """
+ return math.fabs(a - b) < epsilon
+
+ def coords_equal(self, coords_1, coords_2):
+ """Checks if specified coords are equal.
+
+ Args:
+ coords_1 (list): first coords to compare
+ coords_2 (list): second coords to compare
+
+ Returns:
+ bool: True if coords are equal, False otherwise.
+ """
+ return (
+ self.float_equal(coords_1[Axis.X.value], coords_2[Axis.X.value])
+ and self.float_equal(coords_1[Axis.Y.value], coords_2[Axis.Y.value])
+ and self.float_equal(coords_1[Axis.Z.value], coords_2[Axis.Z.value])
+ and self.float_equal(coords_1[Axis.RX.value], coords_2[Axis.RX.value])
+ and self.float_equal(coords_1[Axis.RY.value], coords_2[Axis.RY.value])
+ and self.float_equal(coords_1[Axis.RZ.value], coords_2[Axis.RZ.value])
+ )
+
+ def angles_equal(self, angles_1, angles_2):
+ """Checks if specified angles are equal.
+
+ Args:
+ angles_1 (list): first angles to compare
+ angles_2 (list): second angles to compare
+
+ Returns:
+ bool: True if angles are equal, False otherwise.
+ """
+ return (
+ self.float_equal(angles_1[Joint.J1.value], angles_2[Joint.J1.value])
+ and self.float_equal(angles_1[Joint.J2.value], angles_2[Joint.J2.value])
+ and self.float_equal(angles_1[Joint.J3.value], angles_2[Joint.J3.value])
+ and self.float_equal(angles_1[Joint.J4.value], angles_2[Joint.J4.value])
+ and self.float_equal(angles_1[Joint.J5.value], angles_2[Joint.J5.value])
+ and self.float_equal(angles_1[Joint.J6.value], angles_2[Joint.J6.value])
+ )
+
def string_to_int(self, data):
try:
val = int(data)
@@ -82,6 +197,27 @@ def invalid_coords(self):
coords = [-1.0, -2.0, -3.0, -4.0, -1.0, -1.0]
return coords
+ def detect_robot(self):
+ return int(self.get_analog_in(55))
+
+ def start_robot(self):
+ """Starts robot.
+
+ Returns:
+ bool: currently always returns True
+ """
+ command = "start_robot()\n"
+ res = self.send_command(command)
+ return True
+
+ def recover_robot(self):
+ """Recovers robot after collision.
+
+ Returns:
+ bool: currently always returns True
+ """
+ return self.start_robot()
+
def get_angles(self):
command = "get_angles()\n"
res = self.send_command(command)
@@ -97,30 +233,78 @@ def get_speed(self):
res = self.send_command(command)
return self.string_to_double(res)
- def power_on(self):
+ def _power_on(self):
command = "power_on()\n"
res = self.send_command(command)
return True
- def power_off(self):
+ def _power_off(self):
command = "power_off()\n"
res = self.send_command(command)
return True
- def start_robot(self):
- command = "start_robot()\n"
+ 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 state_check(self):
+ command = "state_check()\n"
res = self.send_command(command)
- return True
+ return res == "1"
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 is_in_position(self, coords, jog_mode):
+ """Returns True if current position equals passed coords.
+
+ Args:
+ coords (list[float]): coords or angles
+ jog_mode (JogMode): JogMode enum value
+
+ Returns:
+ bool: True if robot is in passed coords, False otherwise
+ """
+ if type(jog_mode) is not JogMode:
+ raise TypeError("jog_mode should be of type JogMode")
+ if jog_mode == JogMode.JOG_TELEOP:
+ return self.coords_equal(self.get_coords(), coords)
+ else:
+ return self.angles_equal(self.get_angles(), coords)
+
+ def set_free_move_mode(self, on=True):
+ """Enables or disables free move mode.
+
+ Args:
+ on (bool, optional): True to enable free move mode,
+ False to disable free move mode.
+ Defaults to True.
+ """
+ command = "set_free_move(" + str(int(on)) + ")\n"
+ self.send_command(command)
+
+ def set_payload(self, payload):
+ """Sets payload value on the robot.
+
+ Args:
+ payload (float): payload value (0.0~2.0)
+ """
+ command = "set_payload(" + str(payload) + ")\n"
+ self.send_command(command)
+
+ def get_payload(self):
+ """Returns current payload value.
+
+ Returns:
+ float: payload value
+ """
+ return self.get_analog_out(53)
def upload_file(self, local_filename, remote_filename):
"""Upload local file to robot via socket. Permissions are checked before
@@ -210,18 +394,6 @@ def set_upside_down(self, up_down):
command = "set_upside_down(" + up + ")\n"
self.send_command(command)
- def set_payload(self, payload):
- command = "set_payload(" + 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)
@@ -249,20 +421,52 @@ def get_digital_out(self, pin_number):
res = self.send_command(command)
return self.string_to_int(res)
- def get_joint_current(self, joint_number):
- command = "get_joint_current(" + str(joint_number) + ")\n"
- print(command)
- res = self.send_command(command)
- return self.string_to_double(res)
-
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"
+ def get_analog_in(self, pin_number):
+ """Returns specified analog input pin value.
+
+ Args:
+ pin_number (int): pin number (0-63)
+
+ Returns:
+ float: pin value
+ """
+ command = "get_analog_in(" + str(pin_number) + ")\n"
+ res = self.send_command(command)
+ return self.string_to_double(res)
+
+ def get_analog_out(self, pin_number):
+ """Returns specified analog output pin value.
+
+ Args:
+ pin_number (int): pin number (0-63)
+
+ Returns:
+ float: pin value
+ """
+ command = "get_analog_out(" + str(pin_number) + ")\n"
+ res = self.send_command(command)
+ return self.string_to_double(res)
+
+ def set_analog_out(self, pin_number, pin_value):
+ """Sets specified analog output pin to given value.
+
+ Args:
+ pin_number (int): pin number (0~63).
+ pin_value (float): pin value
+ """
+ command = "set_analog_out(" + str(pin_number) + "," + str(pin_value) + ")\n"
self.send_command(command)
+ def get_joint_current(self, joint_number):
+ command = "get_joint_current(" + str(joint_number) + ")\n"
+ print(command)
+ res = self.send_command(command)
+ return self.string_to_double(res)
+
def send_feed_override(self, override):
command = "set_feed_rate(" + str(override) + ")\n"
res = self.send_command(command)
@@ -372,6 +576,127 @@ def set_gripper_mode(self, mode):
command = "set_gripper_mode(" + str(mode) + ")\n"
return self.send_command(command)
+ def init_ele_gripper(self):
+ """Initializes Electric Gripper.
+
+ Returns:
+ str: return message
+ """
+ command = "init_ele_gripper()\n"
+ return self.send_command(command)
+
+ def set_ele_gripper_open(self):
+ """Fully opens Electric Gripper.
+
+ Returns:
+ str: return message
+ """
+ command = "set_ele_gripper_open()\n"
+ return self.send_command(command)
+
+ def set_ele_gripper_close(self):
+ """Fully closes Electric Gripper.
+
+ Returns:
+ str: return message
+ """
+ command = "set_ele_gripper_close()\n"
+ return self.send_command(command)
+
+ def force_gripper_set_angle(self, angle):
+ """Sets angle of Force-Controlled gripper.
+
+ Args:
+ angle (int): angle, 0-100
+
+ Returns:
+ str: return message
+ """
+ command = "force_gripper_set_angle(" + str(angle) + ")\n"
+ return self.send_command(command)
+
+ def force_gripper_get_angle(self):
+ """Returns current angle of force-controlled gripper.
+
+ Returns:
+ str: return message
+ """
+ command = "force_gripper_get_angle()\n"
+ return self.send_command(command)
+
+ def force_gripper_full_open(self):
+ """Fully opens force-controlled gripper.
+
+ Returns:
+ str: return message
+ """
+ command = "force_gripper_full_open()\n"
+ return self.send_command(command)
+
+ def force_gripper_full_close(self):
+ """Fully closes force-controlled gripper.
+
+ Returns:
+ str: return message
+ """
+ command = "force_gripper_full_close()\n"
+ return self.send_command(command)
+
+ def force_gripper_set_torque(self, torque):
+ """Sets torque of force-controlled gripper.
+
+ Args:
+ torque (int): torque, 0-100 (mapped to 100-300)
+
+ Returns:
+ str: return message
+ """
+ command = "force_gripper_set_torque(" + str(torque) + ")\n"
+ return self.send_command(command)
+
+ def force_gripper_get_torque(self):
+ """Returns current torque of force-controlled gripper.
+
+ Returns:
+ str: return message
+ """
+ command = "force_gripper_get_torque()\n"
+ return self.send_command(command)
+
+ def set_tool_reference(self, tool_reference):
+ """Set tool coordinate system.
+
+ Args:
+ tool_reference (list):
+
+ Returns:
+ _type_: _description_
+ """
+ rotation_matrix = np.eye(3) # No rotation example
+ translation_vector = tool_reference[:3] # Tool offset along z-axis of flange
+ rotation_matrix = CvtEulerAngleToRotationMatrix(tool_reference[3:6]* np.pi/180.0)
+ return transformation_matrix_from_parameters(rotation_matrix, translation_vector)
+
+ def get_tool_coords(self):
+ """Set tool coordinate system.
+
+ Returns:
+ _type_: _description_
+ """
+ current_coords = np.array(self.get_coords())
+ tool_coords = flangeToTool(current_coords, self.tool_matrix)
+ return tool_coords
+
+ def write_tool_coords(self, tool_coords, speed):
+ """Tool coordinate motion control
+
+ Args:
+ tool_coords (list): _description_
+ speed (int): _description_
+ """
+ flange_coords = toolToflange(tool_coords, self.tool_matrix)
+ self.write_coords(flange_coords, speed)
+
if __name__ == "__main__":
ep = ElephantRobot("192.168.124.28", 5001)
@@ -383,8 +708,8 @@ def set_gripper_mode(self, mode):
print(ep.get_angles())
print(ep.get_coords())
print(ep.get_speed())
- # print(ep.power_on())
- # print(ep.power_off())
+ # print(ep._power_on())
+ # print(ep._power_off())
print(ep.check_running())
print(ep.state_check())
print(ep.program_open("a.tax"))
@@ -398,8 +723,8 @@ def set_gripper_mode(self, mode):
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._state_on())
+ print(ep._state_off())
print(ep.task_stop())
print(ep.jog_angle("j2", 1, 300))
print(ep.jog_coord("rY", 0, 200))
diff --git a/pymycobot/end_control.py b/pymycobot/end_control.py
new file mode 100644
index 0000000..9c8d48a
--- /dev/null
+++ b/pymycobot/end_control.py
@@ -0,0 +1,430 @@
+# coding=utf-8
+
+from pymycobot.common import ProtocolCode, ProGripper, FingerGripper
+
+class ForceGripper:
+ def calibration_parameters(self, *args, **kwargs):
+ pass
+ def _mesg(self, *args, **kwargs):
+ pass
+ # 设置力矩手爪
+ def set_pro_gripper(self, gripper_id, gripper_address, value=0, has_return=False):
+ # 调用校准参数函数
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_address=gripper_address)
+ # 发送设置力矩手爪的指令
+ return self._mesg(ProtocolCode.MERCURY_SET_TOQUE_GRIPPER, gripper_id, [gripper_address], [value],
+ has_return=has_return)
+
+ # 获取力矩手爪
+ def get_pro_gripper(self, gripper_id, gripper_address):
+ # 调用校准参数函数
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_address=gripper_address)
+ # 发送获取力矩手爪的指令
+ return self._mesg(ProtocolCode.MERCURY_GET_TOQUE_GRIPPER, gripper_id, [gripper_address])
+
+ # 设置力矩手爪角度
+ def set_pro_gripper_angle(self, gripper_id, gripper_angle):
+ # 调用校准参数函数
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_angle=gripper_angle)
+ # 发送设置力矩手爪角度的指令
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_ANGLE, gripper_angle)
+
+ # 设置力矩手爪打开
+ def set_pro_gripper_open(self, gripper_id):
+ # 调用校准参数函数
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_ANGLE, 100)
+
+ def set_pro_gripper_close(self, gripper_id):
+ # 设置力矩手爪关闭
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ # 调用校准参数函数
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_ANGLE, 0)
+ # 发送设置力矩手爪关闭的指令
+
+ def get_pro_gripper_angle(self, gripper_id):
+ # 获取力矩手爪角度
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ # 调用校准参数函数
+ return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_ANGLE)
+ # 发送获取力矩手爪角度的指令
+
+ def set_pro_gripper_calibration(self, gripper_id):
+ # 设置力矩手爪校准
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ # 调用校准参数函数
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_CALIBRATION)
+ # 发送设置力矩手爪校准的指令
+
+ def get_pro_gripper_status(self, gripper_id):
+ # 获取力矩手爪状态
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ # 调用校准参数函数
+ return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_STATUS)
+ # 发送获取力矩手爪状态的指令
+
+ def set_pro_gripper_torque(self, gripper_id, torque):
+ # 设置力矩手爪扭矩
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id, torque=torque)
+ # 调用校准参数函数
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_TORQUE, torque)
+ # 发送设置力矩手爪扭矩的指令
+
+ def get_pro_gripper_torque(self, gripper_id):
+ # 获取力矩手爪扭矩
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ # 调用校准参数函数
+ return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_TORQUE)
+ # 发送获取力矩手爪扭矩的指令
+
+ def set_pro_gripper_speed(self, gripper_id, speed):
+ # 设置力矩手爪速度
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id, speed=speed)
+ # 调用校准参数函数
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_SPEED, speed)
+ # 发送设置力矩手爪速度的指令
+
+ def get_pro_gripper_speed(self, gripper_id):
+ # 获取力矩手爪速度
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ # 调用校准参数函数
+ return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_SPEED)
+ # 发送获取力矩手爪速度的指令
+
+ def set_pro_gripper_abs_angle(self, gripper_id, angle):
+ # 设置力矩手爪绝对角度
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_ABS_ANGLE, angle, has_return=True)
+ # 发送设置力矩手爪绝对角度的指令
+
+ def set_pro_gripper_pause(self, gripper_id):
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_PAUSE)
+
+ def set_pro_gripper_stop(self, gripper_id):
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_STOP)
+
+ def set_pro_gripper_resume(self, gripper_id):
+ return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_RESUME)
+
+
+class ThreeHand:
+ def calibration_parameters(self, *args, **kwargs):
+ pass
+ def _mesg(self, *args, **kwargs):
+ pass
+ def __set_tool_fittings_value(self, addr, *args, gripper_id=14, **kwargs):
+ kwargs["has_replay"] = True
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.MERCURY_SET_TOQUE_GRIPPER, gripper_id, [addr], *args or ([0x00],), **kwargs)
+
+ def __get_tool_fittings_value(self, addr, *args, gripper_id=14, **kwargs):
+ kwargs["has_replay"] = True
+ return self._mesg(ProtocolCode.MERCURY_GET_TOQUE_GRIPPER, gripper_id, [addr], *args or ([0x00],), **kwargs)
+
+ def get_hand_firmware_major_version(self, gripper_id=14):
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_MAJOR_FIRMWARE_VERSION, gripper_id=gripper_id
+ )
+
+ def get_hand_firmware_minor_version(self, gripper_id=14):
+ return self.__get_tool_fittings_value(FingerGripper.GET_HAND_MINOR_FIRMWARE_VERSION, gripper_id=gripper_id)
+
+ def set_hand_gripper_id(self, new_hand_id, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, new_hand_id=new_hand_id)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_ID, [new_hand_id], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_id(self, gripper_id=14):
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_ID, gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_angle(self, hand_id, gripper_angle, gripper_id=14):
+ """Set the angle of the single joint of the gripper
+
+ Args:
+ hand_id (int): 1 ~ 6
+ gripper_angle (int): 0 ~ 100
+ gripper_id (int) : 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id, gripper_angle=gripper_angle)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_ANGLE, [hand_id], [gripper_angle], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_angle(self, hand_id, gripper_id=14):
+ """Get the angle of the single joint of the gripper
+
+ Args:
+ hand_id (int): 1 ~ 6
+ gripper_id (int) : 1 ~ 254
+
+ Return:
+ gripper_angle (int): 0 ~ 100
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_ANGLE, [hand_id], gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_angles(self, angles, speed, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, speed=speed)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_ANGLES, [angles], [speed], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_angles(self, gripper_id=14):
+ return self.__get_tool_fittings_value(FingerGripper.GET_HAND_ALL_ANGLES, gripper_id)
+
+ def set_hand_gripper_torque(self, hand_id, torque, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id, torque=torque)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_TORQUE, [hand_id], [torque], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_torque(self, hand_id, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_TORQUE, [hand_id], gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_calibrate(self, hand_id, gripper_id=14):
+ """ Setting the gripper jaw zero position
+
+ Args:
+ hand_id (int): 1 ~ 6
+ gripper_id (int): 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_CALIBRATION, [hand_id], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_status(self, gripper_id=14):
+ """ Get the clamping status of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ 0 - Moving
+ 1 - Stopped moving, no clamping detected
+ 2 - Stopped moving, clamping detected
+ 3 - After clamping detected, the object fell
+ """
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_STATUS, gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_enabled(self, flag, gripper_id=14):
+ """ Set the enable state of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ flag (int): 0 or 1
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_ENABLED, [flag], gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_speed(self, hand_id, speed, gripper_id=14):
+ """ Set the speed of the gripper
+
+ Args:
+ hand_id (int): 1 ~ 6
+ speed (int): 1 ~ 100
+ gripper_id (int): 1 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id, speed=speed)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_SPEED, [hand_id], [speed], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_default_speed(self, hand_id, gripper_id=14):
+ """ Get the default speed of the gripper
+
+ Args:
+ hand_id (int): 1 ~ 6
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ default speed (int): 1 ~ 100
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_DEFAULT_SPEED, [hand_id], gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_pinch_action(self, pinch_mode, gripper_id=14):
+ """ Set the pinching action of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ pinch_mode (int):
+ 0 - Index finger and thumb pinch
+ 1 - Middle finger and thumb pinch
+ 2 - Three-finger grip
+ 3 - Two-finger grip
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, pinch_mode=pinch_mode)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_PINCH_ACTION, pinch_mode, gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_p(self, hand_id, value, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_P, [hand_id], [value], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_p(self, hand_id, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_P, [hand_id], gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_d(self, hand_id, value, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_D, [hand_id], [value], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_d(self, hand_id, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_D, [hand_id], gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_i(self, hand_id, value, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_I, [hand_id], [value], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_i(self, hand_id, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_I, [hand_id], gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_min_pressure(self, hand_id, value, gripper_id=14):
+ """ Set the minimum starting force of the single joint of the gripper
+
+ Args:
+ hand_id (int): 1 ~ 6
+ value (int): 0 ~ 254
+ gripper_id (int): 1 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_MIN_PRESSURE, [hand_id], [value], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_min_pressure(self, hand_id, gripper_id=14):
+ """ Set the minimum starting force of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ hand_id (int): 1 ~ 6
+
+ Return:
+ min pressure value (int): 0 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_MIN_PRESSURE, [hand_id], gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_clockwise(self, hand_id, value, gripper_id=14):
+ """
+ state: 0 or 1, 0 - disable, 1 - enable
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_CLOCKWISE, [hand_id], [value], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_clockwise(self, hand_id, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_CLOCKWISE, hand_id, gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_counterclockwise(self, hand_id, value, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_COUNTERCLOCKWISE, [hand_id], [value], gripper_id=gripper_id
+ )
+
+ def get_hand_gripper_counterclockwise(self, hand_id, gripper_id=14):
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_GRIPPER_COUNTERCLOCKWISE, [hand_id], gripper_id=gripper_id
+ )
+
+ def get_hand_single_pressure_sensor(self, hand_id, gripper_id=14):
+ """ Get the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ hand_id (int): 1 ~ 6
+
+ Return:
+ int: 0 ~ 4096
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, hand_id=hand_id)
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_SINGLE_PRESSURE_SENSOR, [hand_id], gripper_id=gripper_id
+ )
+
+ def get_hand_all_pressure_sensor(self, gripper_id=14):
+ """ Get the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ int: 0 ~ 4096
+
+ """
+ return self.__get_tool_fittings_value(
+ FingerGripper.GET_HAND_ALL_PRESSURE_SENSOR, gripper_id=gripper_id
+ )
+
+ def set_hand_gripper_pinch_action_speed_consort(self, pinch_pose, rank_mode, gripper_id=14, idle_flag=None):
+ """ Setting the gripper pinching action-speed coordination
+
+ Args:
+ pinch_pose (int): 0 ~ 4
+ 0: All joints return to zero
+ 1: Index finger and thumb pinch together
+ 2: Middle finger and thumb pinch together
+ 3: Index finger and middle finger pinch together
+ 4: Three fingers together
+ rank_mode (int): 0 ~ 5
+ The degree of closure,the higher the level, the more closed
+ gripper_id (int): 1 ~ 254
+ idle_flag (int): default None or 1
+ Idle flag. By default, there is no such byte. When this byte is 1, the idle finger can be freely manipulated.
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, pinch_pose=pinch_pose, rank_mode=rank_mode, idle_flag=idle_flag)
+ if idle_flag is None:
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT, pinch_pose, rank_mode, gripper_id=gripper_id
+ )
+ else:
+ return self.__set_tool_fittings_value(
+ FingerGripper.SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT, pinch_pose, rank_mode, idle_flag,
+ gripper_id=gripper_id
+ )
\ No newline at end of file
diff --git a/pymycobot/error.py b/pymycobot/error.py
index 5c2ea6a..768ea45 100644
--- a/pymycobot/error.py
+++ b/pymycobot/error.py
@@ -1,7 +1,10 @@
# coding=utf-8
-import json
-import os
+import functools
+import socket
+import traceback
+import re
+from pymycobot.robot_info import RobotLimit
# In order to adapt to the atom side, ID of 0-5 or 1-6 are allowed.
# In order to support end control, ID 7 is allowed.
MIN_ID = 0
@@ -57,6 +60,8 @@ class MyCobot280DataException(Exception):
class MyCobot320DataException(Exception):
pass
+class MercuryRobotException(Exception):
+ pass
def check_boolean(b):
if b != 0 and b != 1:
@@ -87,16 +92,26 @@ def check_value_type(parameter, value_type, exception_class, _type):
"The acceptable parameter {} should be an {}, but the received {}".format(parameter, _type, value_type))
-def check_coords(value, robot_limit, class_name, exception_class):
+def check_coords(parameter_name, value, robot_limit, class_name, exception_class, serial_port=None):
if not isinstance(value, list):
- raise exception_class("`coords` must be a list.")
+ raise exception_class(f"`{parameter_name}` must be a list.")
if len(value) != 6:
- raise exception_class("The length of `coords` must be 6.")
+ raise exception_class(f"The length of `{parameter_name}` must be 6.")
+ if serial_port:
+ if serial_port == "/dev/left_arm":
+ min_coord = robot_limit[class_name]["left_coords_min"]
+ max_coord = robot_limit[class_name]["left_coords_max"]
+ elif serial_port == "/dev/right_arm":
+ min_coord = robot_limit[class_name]["right_coords_min"]
+ max_coord = robot_limit[class_name]["right_coords_max"]
+ else:
+ min_coord = robot_limit[class_name]["coords_min"]
+ max_coord = robot_limit[class_name]["coords_max"]
for idx, coord in enumerate(value):
- if not robot_limit[class_name]["coords_min"][idx] <= coord <= robot_limit[class_name]["coords_max"][idx]:
+ if not min_coord[idx] <= coord <= max_coord[idx]:
raise exception_class(
- "Has invalid coord value, error on index {0}. received {3} .but angle should be {1} ~ {2}.".format(
- idx, robot_limit[class_name]["coords_min"][idx], robot_limit[class_name]["coords_max"][idx], coord
+ "Has invalid coord value, error on index {0}. received {3} .but coord should be {1} ~ {2}.".format(
+ idx, min_coord[idx], max_coord[idx], coord
)
)
@@ -121,10 +136,10 @@ def check_0_or_1(parameter, value, range_data, value_type, exception_class, _typ
check_value_type(parameter, value_type, exception_class, _type)
if value not in range_data:
error = "The data supported by parameter {} is ".format(parameter)
- lens = len(range_data)
- for idx in range(lens):
+ len_data = len(range_data)
+ for idx in range(len_data):
error += str(range_data[idx])
- if idx != lens - 1:
+ if idx != len_data - 1:
error += " or "
error += ", but the received value is {}".format(value)
raise exception_class(error)
@@ -132,8 +147,8 @@ def check_0_or_1(parameter, value, range_data, value_type, exception_class, _typ
def check_id(value, id_list, exception_class):
raise exception_class(
- "The id not right, should be in {0}, but received {1}.".format(
- id_list, value
+ "The joint_id not right, should be in {0}, but received {1}.".format(
+ id_list, value
)
)
@@ -184,7 +199,7 @@ def public_check(parameter_list, kwargs, robot_limit, class_name, exception_clas
# TODO 280/320共用MyCobot，无法进行数据限位
# elif parameter == 'coords':
# check_coords(value, robot_limit, class_name, exception_class)
- elif parameter in ['rftype', 'move_type', 'end', 'is_linear', 'status', 'mode', 'direction']:
+ elif parameter in ['rftype', 'move_type', 'end', 'is_linear', 'status', 'mode', 'direction', 'state', 'deceleration']:
check_0_or_1(parameter, value, [0, 1], value_type, exception_class, int)
elif parameter == 'acceleration':
check_value_type(parameter, value_type, exception_class, int)
@@ -195,10 +210,9 @@ def public_check(parameter_list, kwargs, robot_limit, class_name, exception_clas
elif parameter == "angles":
check_angles(value, robot_limit, class_name, exception_class)
elif parameter == 'angle':
- id = kwargs.get('id', None)
- index = robot_limit[class_name]['id'][id - 1] - 1
- if value < robot_limit[class_name]["angles_min"][index] or value > robot_limit[class_name]["angles_max"][
- index]:
+ joint_id = kwargs.get('id', None)
+ index = robot_limit[class_name]['id'][joint_id-1] - 1
+ if value < robot_limit[class_name]["angles_min"][index] or value > robot_limit[class_name]["angles_max"][index]:
raise exception_class(
"angle value not right, should be {0} ~ {1}, but received {2}".format(
robot_limit[class_name]["angles_min"][index], robot_limit[class_name]["angles_max"][index],
@@ -234,19 +248,19 @@ def public_check(parameter_list, kwargs, robot_limit, class_name, exception_clas
for data in value:
data_type = type(data)
check_value_type(parameter, data_type, exception_class, int)
- if data < 0 or data > 3400:
- raise exception_class("The range of speed is 0 ~ 3400, but the received value is {}".format(data))
+ if data < 0 or data > 6000:
+ raise exception_class("The range of speed is 0 ~ 6000, but the received value is {}".format(data))
elif parameter in ['servo_id_pdi', 'encode_id']:
check_value_type(parameter, value_type, exception_class, int)
if "MyCobot" in class_name or "MechArm" in class_name:
- if value < 1 or value > 6:
- raise exception_class("The range of id is 1 ~ 6, but the received is {}".format(value))
+ if value < 1 or value > 7:
+ raise exception_class("The range of id is 1 ~ 6 or 7, but the received is {}".format(value))
if class_name in ["MyPalletizer", "MyPalletizer260"]:
if value < 1 or value > 4:
raise exception_class("The range of id is 1 ~ 4, but the received is {}".format(value))
elif "MyArm" in class_name or "MyCobot" in class_name or "MechArm" in class_name:
if value < 1 or value > 7:
- raise exception_class("The range of id is 1 ~ 7, but the received is {}".format(value))
+ raise exception_class("The range of joint_id is 1 ~ 7, but the received is {}".format(value))
elif parameter == "torque":
torque_min = 150
torque_max = 980
@@ -261,203 +275,85 @@ def public_check(parameter_list, kwargs, robot_limit, class_name, exception_clas
"The range of current is {} ~ {}, but the received is {}".format(current_min, current_max, value))
elif parameter == 'end_direction':
check_0_or_1(parameter, value, [1, 2, 3], value_type, exception_class, int)
-
-
+ elif parameter == "pin_no":
+ if "Mercury" in class_name:
+ check_0_or_1(parameter, value, [1, 2, 3, 4, 5, 6], value_type, exception_class, int)
def calibration_parameters(**kwargs):
- robot_limit = {
- "Mercury": {
- "id": [1, 2, 3, 4, 5, 6, 7, 11, 12, 13],
- "angles_min": [-178, -74, -178, -180, -178, -20, -180, -60, -140, -120],
- "angles_max": [178, 130, 178, 10, 178, 273, 180, 0, 190, 120],
- "coords_min": [-466, -466, -240, -180, -180, -180],
- "coords_max": [466, 466, 531, 180, 180, 180]
- },
- "MercurySocket": {
- "id": [1, 2, 3, 4, 5, 6, 7, 11, 12, 13],
- "angles_min": [-178, -74, -178, -180, -178, -20, -180, -60, -140, -120],
- "angles_max": [178, 130, 178, 10, 178, 273, 180, 0, 190, 120],
- "coords_min": [-466, -466, -240, -180, -180, -180],
- "coords_max": [466, 466, 531, 180, 180, 180]
- },
- "MyCobot": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-168, -135, -150, -145, -165, -180],
- "angles_max": [168, 135, 150, 145, 165, 180],
- "coords_min": [-350, -350, -70, -180, -180, -180],
- "coords_max": [350, 350, 523.9, 180, 180, 180]
- },
- "MyCobotSocket": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-168, -135, -150, -145, -165, -180],
- "angles_max": [168, 135, 150, 145, 165, 180],
- "coords_min": [-350, -350, -70, -180, -180, -180],
- "coords_max": [350, 350, 523.9, 180, 180, 180]
- },
- "MyCobot280": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-168, -135, -150, -145, -165, -180],
- "angles_max": [168, 135, 150, 145, 165, 180],
- "coords_min": [-350, -350, -70, -180, -180, -180],
- "coords_max": [350, 350, 523.9, 180, 180, 180]
- },
- "MyCobot280Socket": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-168, -135, -150, -145, -165, -180],
- "angles_max": [168, 135, 150, 145, 165, 180],
- "coords_min": [-350, -350, -70, -180, -180, -180],
- "coords_max": [350, 350, 523.9, 180, 180, 180]
- },
- "MyCobot320": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-170, -137, -151, -148, -169, -180],
- "angles_max": [170, 137, 142, 148, 169, 180],
- "coords_min": [-350, -350, -41, -180, -180, -180],
- "coords_max": [350, 350, 523.9, 180, 180, 180]
- },
- "MyCobot320Socket": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-170, -137, -151, -148, -169, -180],
- "angles_max": [170, 137, 142, 148, 169, 180],
- "coords_min": [-350, -350, -41, -180, -180, -180],
- "coords_max": [350, 350, 523.9, 180, 180, 180]
- },
- "MechArm": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-165, -90, -180, -165, -115, -175],
- "angles_max": [165, 90, 70, 165, 115, 175],
- "coords_min": [-272, -272, -36, -180, -180, -180],
- "coords_max": [272, 272, 408.9, 180, 180, 180]
- },
- "MechArm270": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-165, -90, -180, -165, -115, -175],
- "angles_max": [165, 90, 70, 165, 115, 175],
- "coords_min": [-272, -272, -36, -180, -180, -180],
- "coords_max": [272, 272, 408.9, 180, 180, 180]
- },
- "MechArmSocket": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-165, -90, -180, -165, -115, -175],
- "angles_max": [165, 90, 70, 165, 115, 175],
- "coords_min": [-272, -272, -36, -180, -180, -180],
- "coords_max": [272, 272, 408.9, 180, 180, 180]
- },
- "MyArm": {
- "id": [1, 2, 3, 4, 5, 6, 7, 8],
- "angles_min": [-160, -70, -170, -113, -170, -115, -180],
- "angles_max": [160, 115, 170, 75, 170, 115, 180],
- "coords_min": [-310, -310, -140, -180, -180, -180],
- "coords_max": [310, 310, 480, 180, 180, 180]
- },
- "MyArmSocket": {
- "id": [1, 2, 3, 4, 5, 6, 7, 8],
- "angles_min": [-160, -70, -170, -113, -170, -115, -180],
- "angles_max": [160, 115, 170, 75, 170, 115, 180],
- "coords_min": [-310, -310, -140, -180, -180, -180],
- "coords_max": [310, 310, 480, 180, 180, 180]
- },
- "MyPalletizer": {
- "id": [1, 2, 3, 4, 7],
- "angles_min": [-162, -2, -92, -180],
- "angles_max": [162, 90, 60, 180],
- "coords_min": [-260, -260, -15, -180],
- "coords_max": [260, 260, 357.58, 180]
- },
- "MyPalletizer260": {
- "id": [1, 2, 3, 4, 7],
- "angles_min": [-162, -2, -92, -180],
- "angles_max": [162, 90, 60, 180],
- "coords_min": [-260, -260, -15, -180],
- "coords_max": [260, 260, 357.58, 180]
- },
- "MyPalletizerSocket": {
- "id": [1, 2, 3, 4, 7],
- "angles_min": [-162, -2, -92, -180],
- "angles_max": [162, 90, 60, 180],
- "coords_min": [-260, -260, -15, -180],
- "coords_max": [260, 260, 357.58, 180]
- },
- "UltraArm": {
- "id": [1, 2, 3, 4, 7],
- "angles_min": [-150, -20, -5, -180],
- "angles_max": [170, 90, 110, 180],
- "coords_min": [-340, -340, 0, -180],
- "coords_max": [340, 340, 270.58, 180]
- },
- "MyBuddy": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-165, -165, -165, -165, -165, -175],
- "angles_max": [165, 165, 165, 165, 165, 175],
- "waist_angle_min": -120,
- "waist_angle_max": 120,
- "left_coords_min": [0, -40, 0, -180, -180, -180],
- "left_coords_max": [250, 260, 480, 180, 180, 180],
- "right_coords_min": [0, -260, 0, -180, -180, -180],
- "right_coords_max": [250, 40, 480, 180, 180, 180]
- },
- "MyBuddySocket": {
- "id": [1, 2, 3, 4, 5, 6, 7],
- "angles_min": [-165, -165, -165, -165, -165, -175],
- "angles_max": [165, 165, 165, 165, 165, 175],
- "waist_angle_min": -120,
- "waist_angle_max": 120,
- "left_coords_min": [0, -40, 0, -180, -180, -180],
- "left_coords_max": [250, 260, 480, 180, 180, 180],
- "right_coords_min": [0, -260, 0, -180, -180, -180],
- "right_coords_max": [250, 40, 480, 180, 180, 180]
- }
- }
+ # with open(os.path.dirname(os.path.abspath(__file__))+"/robot_limit.json") as f:
+ robot_limit = RobotLimit.robot_limit
parameter_list = list(kwargs.keys())
class_name = kwargs.get("class_name", None)
if class_name in ["Mercury", "MercurySocket"]:
for parameter in parameter_list[1:]:
value = kwargs.get(parameter, None)
- if parameter == 'id' and value not in robot_limit[class_name][parameter]:
- check_id(value, robot_limit[class_name][parameter], MercuryDataException)
+ if parameter == 'joint_id':
+ if value not in robot_limit[class_name][parameter]:
+ check_id(value, robot_limit[class_name][parameter], MercuryDataException)
elif parameter == 'angle':
- id = kwargs.get('id', None)
- if id in [11, 12, 13]:
- index = robot_limit[class_name]['id'][id - 4] - 4
+ joint_id = kwargs.get('joint_id', None)
+ if joint_id in [11,12,13]:
+ index = robot_limit[class_name]['joint_id'][joint_id-4] - 4
else:
- index = robot_limit[class_name]['id'][id - 1] - 1
- if value < robot_limit[class_name]["angles_min"][index] or value > \
- robot_limit[class_name]["angles_max"][index]:
+ index = robot_limit[class_name]['joint_id'][joint_id-1] - 1
+ if value < robot_limit[class_name]["angles_min"][index] or value > robot_limit[class_name]["angles_max"][index]:
raise MercuryDataException(
"angle value not right, should be {0} ~ {1}, but received {2}".format(
robot_limit[class_name]["angles_min"][index], robot_limit[class_name]["angles_max"][index],
value
)
)
- # elif parameter == 'angles':
- # if len(value) not in [7, 10]:
- # raise MercuryDataException("The length of `angles` must be 7 or 10.")
- # for idx, angle in enumerate(value):
- # if not MIN_ANGLE <= angle <= MAX_ANGLE:
- # raise MercuryDataException(
- # "Has invalid angle value, error on index {0}. angle should be {1} ~ {2}.".format(
- # idx, MIN_ANGLE, MAX_ANGLE
- # )
- # )
+ elif parameter == 'angles':
+ if len(value) not in [7, 10]:
+ raise MercuryDataException("The length of `angles` must be 7 or 10.")
+ for idx, angle in enumerate(value):
+ joint_id = idx+1
+ angle_min = robot_limit[class_name]["angles_min"][idx]
+ angle_max = robot_limit[class_name]["angles_max"][idx]
+ if angle < angle_min or angle > angle_max:
+ raise MercuryDataException(
+ "Joint {} angle value of {} exceeds the limit, with a limit range of {} ~ {}.".format(joint_id, angle, angle_min, angle_max)
+ )
elif parameter == 'coord':
-
- index = kwargs.get('id', None) - 1
- if value < robot_limit[class_name]["coords_min"][index] or value > \
- robot_limit[class_name]["coords_max"][index]:
+
+ index = kwargs.get('coord_id', None) - 1
+ if value < robot_limit[class_name]["coords_min"][index] or value > robot_limit[class_name]["coords_max"][index]:
raise MercuryDataException(
- "coord value not right, should be {0} ~ {1}, but received {2}".format(
- robot_limit[class_name]["coords_min"][index], robot_limit[class_name]["coords_max"][index],
- value
+ "The `coord` value of {} exceeds the limit, and the limit range is {} ~ {}".format(
+ value, robot_limit[class_name]["coords_min"][index], robot_limit[class_name]["coords_max"][index]
)
)
- elif parameter == 'coords':
- check_coords(value, robot_limit, class_name, MercuryDataException)
+ elif parameter == 'base_coord':
+ coord_id = kwargs.get('coord_id', None)
+
+ if isinstance(coord_id, int):
+ index = coord_id - 1
+ serial_port = kwargs.get('serial_port', None)
+ if serial_port == "/dev/left_arm":
+ min_coord = robot_limit[class_name]["left_coords_min"][index]
+ max_coord = robot_limit[class_name]["left_coords_max"][index]
+ elif serial_port == "/dev/right_arm":
+ min_coord = robot_limit[class_name]["right_coords_min"][index]
+ max_coord = robot_limit[class_name]["right_coords_max"][index]
+ else:
+ min_coord = robot_limit[class_name]["coords_min"][index]
+ max_coord = robot_limit[class_name]["coords_max"][index]
+ if value < min_coord or value > max_coord:
+ raise MercuryDataException(
+ "The `base_coord` value of {} exceeds the limit, and the limit coord is {} ~ {}".format(
+ value, min_coord, max_coord
+ )
+ )
+ elif parameter in ['coords', 'base_coords']:
+ serial_port = kwargs.get('serial_port', None)
+ check_coords(parameter, value, robot_limit, class_name, MercuryDataException, serial_port)
- elif parameter == 'speed' and not 1 <= value <= 100:
- raise MercuryDataException(
- "speed value not right, should be 1 ~ 100, the error speed is %s"
- % value
- )
+ elif parameter == 'speed':
+ if not 1 <= value <= 100:
+ raise MercuryDataException(
+ "speed value not right, should be 1 ~ 100, the error speed is %s"
+ % value
+ )
elif parameter == 'rgb':
check_rgb_value(value, MercuryDataException, class_name)
@@ -489,8 +385,71 @@ def calibration_parameters(**kwargs):
"The parameter data_len data range only supports 1 ~ 45, but received {}".format(value))
elif parameter == "max_time":
if value < 0:
- raise MercuryDataException(
- "The parameter max_time must be greater than or equal to 0, but received {}".format(value))
+ raise MercuryDataException("The parameter max_time must be greater than or equal to 0, but received {}".format(value))
+ elif parameter == "limit_mode":
+ if value not in [1,2]:
+ raise MercuryDataException("The parameter {} only supports 1, 2, but received {}".format(parameter, value))
+ elif parameter == "_type":
+ if value not in [1,2,3,4]:
+ raise MercuryDataException("The parameter {} only supports 1, 2, 3, 4, but received {}".format(parameter, value))
+ elif parameter == "axis":
+ if value not in [1,2,3]:
+ raise MercuryDataException("The parameter {} only supports 1, 2, 3, but received {}".format(parameter, value))
+ elif parameter == "threshold_value":
+ if value < 50 or value > 250:
+ raise MercuryDataException("The parameter {} only supports 50 ~ 250, but received {}".format(parameter, value))
+ elif parameter == "comp_value":
+ if value < 0 or value > 250:
+ raise MercuryDataException("The parameter {} only supports 0 ~ 250, but received {}".format(parameter, value))
+ elif parameter == "shoot_value":
+ if value < -300 or value > 300:
+ raise MercuryDataException("The parameter {} only supports -300 ~ 300, but received {}".format(parameter, value))
+ elif parameter == "head_id":
+ if value not in [11,12]:
+ raise MercuryDataException("The parameter {} only supports 11, 12, but received {}".format(parameter, value))
+ elif parameter == "err_angle":
+ if value < 0 or value > 5:
+ raise MercuryDataException("The parameter {} only supports 0 ~ 5, but received {}".format(parameter, value))
+ elif parameter == "r":
+ if value < 0 or value > 655.5:
+ raise MercuryDataException("The parameter {} only supports 0 ~ 655.5, but received {}".format(parameter, value))
+ elif parameter == "rank":
+ if value not in [0,1,2]:
+ raise MercuryDataException("The parameter {} only supports 0 or 1 or 2, but received {}".format(parameter, value))
+ elif parameter == "move_type":
+ if value not in [0, 1, 2, 3, 4]:
+ raise MercuryDataException("The parameter {} only supports 0 or 4, but received {}".format(parameter, value))
+ elif parameter == "trajectory":
+ if value not in [0,1,2,3,4]:
+ raise MercuryDataException("The parameter {} only supports [0,1,2,3,4], but received {}".format(parameter, value))
+ elif parameter in ["gripper_id", "new_hand_id"]:
+ if value < 1 or value > 254:
+ raise MercuryDataException("The parameter {} only supports 1 ~ 254, but received {}".format(parameter, value))
+ elif parameter == "gripper_address":
+ if value < 1 or value > 44 or value in [15,17,19]:
+ raise MercuryDataException("The parameter {} only supports 1 ~ 44 (except 15, 17, and 19), but received {}".format(parameter, value))
+ elif parameter == "gripper_angle":
+ if value < 0 or value > 100:
+ raise MercuryDataException("The parameter {} only supports 0 ~ 100, but received {}".format(parameter, value))
+ elif parameter == "torque":
+ if value < 1 or value > 100:
+ raise MercuryDataException("The parameter {} only supports 1 ~ 100, but received {}".format(parameter, value))
+ elif parameter == "hand_id":
+ if value < 1 or value > 6:
+ raise MercuryDataException("The parameter {} only supports 1 ~ 6, but received {}".format(parameter, value))
+ elif parameter == 'pinch_mode':
+ check_0_or_1(parameter, value, [0, 1, 2, 3], value_type, MercuryDataException, int)
+ elif parameter == "pinch_pose":
+ if value < 0 or value > 4:
+ raise MercuryDataException("The parameter {} only supports 0 ~ 4, but received {}".format(parameter, value))
+ elif parameter == "rank_mode":
+ if value < 0 or value > 5:
+ raise MercuryDataException("The parameter {} only supports 0 ~ 5, but received {}".format(parameter, value))
+ elif parameter == "idle_flag":
+ if value != 1:
+ raise MercuryDataException("The parameter {} only supports 1, but received {}".format(parameter, value))
+ else:
+ public_check(parameter_list, kwargs, robot_limit, class_name, MercuryDataException)
elif class_name == "MyAgv":
for parameter in parameter_list[1:]:
value = kwargs.get(parameter, None)
@@ -524,6 +483,9 @@ def calibration_parameters(**kwargs):
value_type = type(value)
if parameter == 'id' and value not in robot_limit[class_name][parameter]:
check_id(value, robot_limit[class_name][parameter], MyCobot280DataException)
+ elif parameter == 'servo_data_id' and value not in [1, 2, 3, 4, 5, 6, 7]:
+ raise MyCobot280DataException(
+ "The id not right, should be in {0}, but received {1}.".format([1, 2, 3, 4, 5, 6, 7], value))
elif parameter == 'rgb':
check_rgb_value(value, MyCobot280DataException, class_name)
elif parameter == 'value':
@@ -550,9 +512,11 @@ def calibration_parameters(**kwargs):
elif parameter == 'flag':
check_0_or_1(parameter, value, [0, 1, 254], value_type, MyCobot280DataException, int)
elif parameter == 'gripper_type':
- check_0_or_1(parameter, value, [1, 3, 4], value_type, MyCobot280DataException, int)
+ if value is not None:
+ check_0_or_1(parameter, value, [1, 3, 4, 5], value_type, MyCobot280DataException, int)
elif parameter == '_type_1':
- check_0_or_1(parameter, value, [1, 2, 3, 4], value_type, MyCobot280DataException, int)
+ if value is not None:
+ check_0_or_1(parameter, value, [1, 2, 3, 4, 5], value_type, MyCobot280DataException, int)
elif parameter == 'gripper_value':
check_value_type(parameter, value_type, MyCobot280DataException, int)
if value < 0 or value > 100:
@@ -561,7 +525,7 @@ def calibration_parameters(**kwargs):
elif parameter in ['account', 'password']:
check_value_type(parameter, value_type, MyCobot280DataException, str)
elif parameter == 'coords':
- check_coords(value, robot_limit, class_name, MyCobot280DataException)
+ check_coords(parameter, value, robot_limit, class_name, MyCobot280DataException)
elif parameter in ['rftype', 'move_type', 'end', 'is_linear', 'status', 'mode', 'direction']:
check_0_or_1(parameter, value, [0, 1], value_type, MyCobot280DataException, int)
elif parameter == 'acceleration':
@@ -614,13 +578,13 @@ def calibration_parameters(**kwargs):
for data in value:
data_type = type(data)
check_value_type(parameter, data_type, MyCobot280DataException, int)
- if data < 0 or data > 3400:
+ if data < 0 or data > 6000:
raise MyCobot280DataException(
- "The range of speed is 0 ~ 3400, but the received value is {}".format(data))
+ "The range of speed is 0 ~ 6000, but the received value is {}".format(data))
elif parameter in ['servo_id_pdi', 'encode_id']:
check_value_type(parameter, value_type, MyCobot280DataException, int)
- if value < 1 or value > 6:
- raise MyCobot280DataException("The range of id is 1 ~ 6, but the received is {}".format(value))
+ if value < 1 or value > 7:
+ raise MyCobot280DataException("The range of id is 1 ~ 6 or 7, but the received is {}".format(value))
elif parameter == "torque":
torque_min = 150
torque_max = 980
@@ -636,12 +600,18 @@ def calibration_parameters(**kwargs):
value))
elif parameter == 'end_direction':
check_0_or_1(parameter, value, [1, 2, 3], value_type, MyCobot280DataException, int)
+ elif parameter == 'is_torque':
+ if value is not None:
+ check_0_or_1(parameter, value, [0, 1], value_type, MyCobot280DataException, int)
elif class_name in ["MyCobot320", "MyCobot320Socket"]:
for parameter in parameter_list[1:]:
value = kwargs.get(parameter, None)
value_type = type(value)
if parameter == 'id' and value not in robot_limit[class_name][parameter]:
check_id(value, robot_limit[class_name][parameter], MyCobot320DataException)
+ elif parameter == 'servo_data_id' and value not in [1, 2, 3, 4, 5, 6, 7]:
+ raise MyCobot320DataException(
+ "The id not right, should be in {0}, but received {1}.".format([1, 2, 3, 4, 5, 6, 7], value))
elif parameter == 'rgb':
check_rgb_value(value, MyCobot320DataException, class_name)
elif parameter == 'value':
@@ -679,7 +649,7 @@ def calibration_parameters(**kwargs):
elif parameter in ['account', 'password']:
check_value_type(parameter, value_type, MyCobot320DataException, str)
elif parameter == 'coords':
- check_coords(value, robot_limit, class_name, MyCobot320DataException)
+ check_coords(parameter, value, robot_limit, class_name, MyCobot320DataException)
elif parameter in ['rftype', 'move_type', 'end', 'is_linear', 'status', 'mode', 'direction']:
check_0_or_1(parameter, value, [0, 1], value_type, MyCobot320DataException, int)
elif parameter == 'acceleration':
@@ -732,13 +702,13 @@ def calibration_parameters(**kwargs):
for data in value:
data_type = type(data)
check_value_type(parameter, data_type, MyCobot320DataException, int)
- if data < 0 or data > 3400:
+ if data < 0 or data > 6000:
raise MyCobot320DataException(
- "The range of speed is 0 ~ 3400, but the received value is {}".format(data))
+ "The range of speed is 0 ~ 6000, but the received value is {}".format(data))
elif parameter in ['servo_id_pdi', 'encode_id']:
check_value_type(parameter, value_type, MyCobot320DataException, int)
- if value < 1 or value > 6:
- raise MyCobot320DataException("The range of id is 1 ~ 6, but the received is {}".format(value))
+ if value < 1 or value > 7:
+ raise MyCobot320DataException("The range of id is 1 ~ 6 or 7, but the received is {}".format(value))
elif parameter == "torque":
torque_min = 150
torque_max = 980
@@ -767,7 +737,7 @@ def calibration_parameters(**kwargs):
raise MyCobot320DataException(
"The range of 'gripper_angle' in {} is 0 ~ 100, but the received value is {}".format(parameter,
gripper_angle))
- elif parameter == "gripper_id":
+ elif parameter == "gripper_id" or parameter == "set_id":
check_value_type(parameter, value_type, MyCobot320DataException, int)
if value < 1 or value > 254:
raise MyCobot320DataException(
@@ -782,92 +752,152 @@ def calibration_parameters(**kwargs):
raise MyCobot320DataException(
"The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
gripper_id))
- if torque_value < 100 or torque_value > 300:
+ if torque_value < 0 or torque_value > 100:
raise MyCobot320DataException(
- "The range of 'torque_value' in {} is 100 ~ 300, but the received value is {}".format(parameter,
+ "The range of 'torque_value' in {} is 0 ~ 100, but the received value is {}".format(parameter,
torque_value))
- elif parameter == "gripper_speed":
- gripper_id, speed = value
- if not isinstance(gripper_id, int) or not isinstance(speed, int):
- raise MyCobot320DataException(
- "Both 'gripper_id' and 'speed' in {} must be integers".format(parameter))
- if gripper_id < 1 or gripper_id > 254:
- raise MyCobot320DataException(
- "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
- gripper_id))
- if speed < 1 or torque_value > 100:
- raise MyCobot320DataException(
- "The range of 'speed' in {} is 1 ~ 100, but the received value is {}".format(parameter,
- speed))
- elif parameter == "set_gripper_args":
- if len(value) != 3:
- raise ValueError(f"Expected 3 arguments, but got {len(value)}")
- gripper_id, address, data = value
- if not isinstance(gripper_id, int) or not isinstance(address, int) or not isinstance(data, int):
- raise MyCobot320DataException(
- "All arguments in {} must be integers".format(parameter))
- if gripper_id < 1 or gripper_id > 254:
+ elif parameter == "gripper_speed":
+ gripper_id, speed = value
+ if not isinstance(gripper_id, int) or not isinstance(speed, int):
+ raise MyCobot320DataException(
+ "Both 'gripper_id' and 'speed' in {} must be integers".format(parameter))
+ if gripper_id < 1 or gripper_id > 254:
+ raise MyCobot320DataException(
+ "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
+ gripper_id))
+ if speed < 1 or speed > 100:
+ raise MyCobot320DataException(
+ "The range of 'speed' in {} is 1 ~ 100, but the received value is {}".format(parameter,
+ speed))
+
+ elif parameter == "set_gripper_args":
+ if len(value) != 3:
+ raise ValueError(f"Expected 3 arguments, but got {len(value)}")
+ gripper_id, address, data = value
+ if not isinstance(gripper_id, int) or not isinstance(address, int) or not isinstance(data, int):
+ raise MyCobot320DataException(
+ "All arguments in {} must be integers".format(parameter))
+ if gripper_id < 1 or gripper_id > 254:
+ raise MyCobot320DataException(
+ "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
+ gripper_id))
+ invalid_addresses = [1, 2, 4, 5, 6, 7, 8, 9, 12, 14, 15, 16, 17, 18, 19, 20, 22, 24, 26, 28, 33, 34, 35,
+ 40, 42, 44]
+ if address < 1 or address > 44:
+ raise MyCobot320DataException(
+ "The range of 'address' in {} is 1 ~ 44, but the received value is {}".format(parameter,
+ address))
+ if address in invalid_addresses:
+ raise MyCobot320DataException(
+ "'address' in {} cannot be one of the following values: {}, but the received value is {}".format(
+ parameter, invalid_addresses, address))
+ # 根据 address 来处理 value
+ if address in [3, 43]:
+ if data < 1 or data > 254:
raise MyCobot320DataException(
- "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
- gripper_id))
- invalid_addresses = [1, 2, 4, 5, 6, 7, 8, 9, 11, 12, 13, 14, 16, 18, 20, 22, 24, 26, 27, 28, 32, 33,
- 36,
- 37, 38, 39, 40, 42, 44]
- if address < 1 or address > 44:
+ "Error in parameter '{}': The range of 'value' for address={} is 1 ~ 254, but the received value is {}".format(
+ parameter, address, data))
+ elif address == 10:
+ if data not in [0, 1]:
raise MyCobot320DataException(
- "The range of 'address' in {} is 1 ~ 44, but the received value is {}".format(parameter,
- address))
- if address in invalid_addresses:
+ "Error in parameter '{}': Value for address={} must be 0 or 1, but the received value is {}".format(
+ parameter, address, data))
+ elif address in [25]:
+ if data < 0 or data > 254:
raise MyCobot320DataException(
- "'address' in {} cannot be one of the following values: {}, but the received value is {}".format(
- parameter, invalid_addresses, address))
- # 根据 address 来处理 value
- if address in [3, 43]:
- if data < 1 or data > 254:
- raise MyCobot320DataException(
- "Error in parameter '{}': The range of 'value' for address={} is 1 ~ 254, but the received value is {}".format(
- parameter, address, data))
- elif address == 10:
- if data not in [0, 1]:
- raise MyCobot320DataException(
- "Error in parameter '{}': Value for address={} must be 0 or 1, but the received value is {}".format(
- parameter, address, data))
- elif address in [15, 17, 19, 25]:
- if data < 0 or data > 254:
- raise MyCobot320DataException(
- "Error in parameter '{}': The range of 'value' for address={} is 0 ~ 254, but the received value is {}".format(
- parameter, address, data))
- elif address in [21, 23]:
- if data < 0 or data > 16:
- raise MyCobot320DataException(
- "Error in parameter '{}': The range of 'value' for address={} is 0 ~ 16, but the received value is {}".format(
- parameter, address, data))
- elif address == 41:
- if data < 0 or data > 100:
- raise MyCobot320DataException(
- "Error in parameter '{}': The range of 'value' for address={} is 0 ~ 100, but the received value is {}".format(
- parameter, address, data))
-
- elif parameter == "get_gripper_args":
- gripper_id, address = value
- if not isinstance(gripper_id, int) or not isinstance(address, int):
+ "Error in parameter '{}': The range of 'value' for address={} is 0 ~ 254, but the received value is {}".format(
+ parameter, address, data))
+ elif address in [21, 23]:
+ if data < 0 or data > 16:
raise MyCobot320DataException(
- "All arguments in {} must be integers".format(parameter))
- if gripper_id < 1 or gripper_id > 254:
+ "Error in parameter '{}': The range of 'value' for address={} is 0 ~ 16, but the received value is {}".format(
+ parameter, address, data))
+ elif address == 41:
+ if data < 0 or data > 100:
raise MyCobot320DataException(
- "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
- gripper_id))
- invalid_addresses = [3, 5, 6, 10, 15, 17, 19, 21, 23, 25, 29, 30, 31, 34, 35, 41, 43]
- if address < 1 or address > 44:
+ "Error in parameter '{}': The range of 'value' for address={} is 0 ~ 100, but the received value is {}".format(
+ parameter, address, data))
+
+ elif parameter == "get_gripper_args":
+ gripper_id, address = value
+ if not isinstance(gripper_id, int) or not isinstance(address, int):
+ raise MyCobot320DataException(
+ "All arguments in {} must be integers".format(parameter))
+ if gripper_id < 1 or gripper_id > 254:
+ raise MyCobot320DataException(
+ "The range of 'gripper_id' in {} is 1 ~ 254, but the received value is {}".format(parameter,
+ gripper_id))
+ invalid_addresses = [3, 5, 6, 10, 11, 13, 15, 17, 19, 21, 23, 25, 27, 29, 30, 31, 32, 36, 37,
+ 38, 39, 41, 43]
+ if address < 1 or address > 44:
+ raise MyCobot320DataException(
+ "The range of 'address' in {} is 1 ~ 44, but the received value is {}".format(parameter,
+ address))
+ if address in invalid_addresses:
+ raise MyCobot320DataException(
+ "'address' in {} cannot be one of the following values: {}, but the received value is {}".format(
+ parameter, invalid_addresses, address))
+
+ elif parameter == "gripper_joint_id":
+ check_value_type(parameter, value_type, MyCobot320DataException, int)
+ if value < 1 or value > 6:
+ raise MyCobot320DataException(
+ "The range of 'gripper_id' in {} is 1 ~ 6, but the received value is {}".format(parameter,
+ value))
+ elif parameter == "gripper_angles":
+ if len(value) != 6:
+ raise MyCobot320DataException(
+ "{}: 'gripper_angles' must contain exactly 6 values, but received {}.".format(class_name,
+ len(value)))
+
+ for i, angle in enumerate(value):
+ if not isinstance(angle, int):
raise MyCobot320DataException(
- "The range of 'address' in {} is 1 ~ 44, but the received value is {}".format(parameter,
- address))
- if address in invalid_addresses:
+ "{}: Value at position {} in 'gripper_angles' must be an integer, but received {}.".format(
+ class_name, i + 1, angle)
+ )
+ if angle < 0 or angle > 100:
raise MyCobot320DataException(
- "'address' in {} cannot be one of the following values: {}, but the received value is {}".format(
- parameter, invalid_addresses, address))
- elif class_name in ["MechArm", "MechArmSocket"]:
+ "{}: Value {} at position {} in 'gripper_angles' is out of range (0 ~ 100).".format(
+ class_name, angle, i + 1)
+ )
+ elif parameter == 'pinch_mode':
+ check_0_or_1(parameter, value, [0, 1, 2, 3], value_type, MyCobot320DataException, int)
+ elif parameter == "gripper_finger_id":
+ check_value_type(parameter, value_type, MyCobot320DataException, int)
+ if value < 1 or value > 5:
+ raise MyCobot320DataException(
+ "The range of 'gripper_id' in {} is 1 ~ 6, but the received value is {}".format(parameter,
+ value))
+ elif parameter == "clockwise":
+ check_value_type(parameter, value_type, MyCobot320DataException, int)
+ if value < 0 or value > 16:
+ raise MyCobot320DataException(
+ "The range of 'value' in {} is 0 ~ 16, but the received value is {}".format(parameter,
+ value))
+ elif parameter in ["min_pressure", "gripper_i"]:
+ check_value_type(parameter, value_type, MyCobot320DataException, int)
+ if value < 0 or value > 254:
+ raise MyCobot320DataException(
+ "The range of 'value' in {} is 0 ~ 254, but the received value is {}".format(parameter,
+ value))
+ elif parameter in ["gripper_p", "gripper_d"]:
+ check_value_type(parameter, value_type, MyCobot320DataException, int)
+ if value < 0 or value > 150:
+ raise MyCobot320DataException(
+ "The range of 'value' in {} is 0 ~ 150, but the received value is {}".format(parameter,
+ value))
+ elif parameter == 'pinch_pose':
+ check_0_or_1(parameter, value, [0, 1, 2, 3, 4], value_type, MyCobot320DataException, int)
+
+ elif parameter == 'rank_mode':
+ check_0_or_1(parameter, value, [0, 1, 2, 3, 4, 5], value_type, MyCobot320DataException, int)
+
+ elif parameter == 'idle_flag':
+ check_0_or_1(parameter, value, [0, 1, 2, 3, 4], value_type, MyCobot320DataException, int)
+
+ elif class_name in ["MechArm"]:
public_check(parameter_list, kwargs, robot_limit, class_name, MechArmDataException)
elif class_name in ["MechArm270", "MechArmSocket"]:
for parameter in parameter_list[1:]:
@@ -875,6 +905,9 @@ def calibration_parameters(**kwargs):
value_type = type(value)
if parameter == 'id' and value not in robot_limit[class_name][parameter]:
check_id(value, robot_limit[class_name][parameter], MechArmDataException)
+ elif parameter == 'servo_data_id' and value not in [1, 2, 3, 4, 5, 6, 7]:
+ raise MechArmDataException(
+ "The id not right, should be in {0}, but received {1}.".format([1, 2, 3, 4, 5, 6, 7], value))
elif parameter == 'rgb':
check_rgb_value(value, MechArmDataException, class_name)
elif parameter == 'value':
@@ -899,11 +932,14 @@ def calibration_parameters(**kwargs):
% value
)
elif parameter == 'flag':
- check_0_or_1(parameter, value, [0, 1, 254], value_type, MechArmDataException, int)
+ if value is not None:
+ check_0_or_1(parameter, value, [0, 1, 254], value_type, MechArmDataException, int)
elif parameter == 'gripper_type':
- check_0_or_1(parameter, value, [1, 3, 4], value_type, MechArmDataException, int)
+ if value is not None:
+ check_0_or_1(parameter, value, [1, 3, 4, 5], value_type, MechArmDataException, int)
elif parameter == '_type_1':
- check_0_or_1(parameter, value, [1, 2, 3, 4], value_type, MechArmDataException, int)
+ if value is not None:
+ check_0_or_1(parameter, value, [1, 2, 3, 4, 5], value_type, MechArmDataException, int)
# if value not in [0, 1, 10]:
# raise exception_class("The data supported by parameter {} is 0 or 1 or 10, but the received value is {}".format(parameter, value))
elif parameter == 'gripper_value':
@@ -936,7 +972,7 @@ def calibration_parameters(**kwargs):
)
)
elif parameter == "coords":
- check_coords(value, robot_limit, class_name, MechArmDataException)
+ check_coords(parameter, value, robot_limit, class_name, MechArmDataException)
elif parameter == 'coord':
id = kwargs.get('id', None)
index = robot_limit[class_name]['id'][id - 1] - 1 # Get the index based on the ID
@@ -966,13 +1002,13 @@ def calibration_parameters(**kwargs):
for data in value:
data_type = type(data)
check_value_type(parameter, data_type, MechArmDataException, int)
- if data < 0 or data > 3400:
+ if data < 0 or data > 6000:
raise MechArmDataException(
- "The range of speed is 0 ~ 3400, but the received value is {}".format(data))
+ "The range of speed is 0 ~ 6000, but the received value is {}".format(data))
elif parameter in ['servo_id_pdi', 'encode_id']:
check_value_type(parameter, value_type, MechArmDataException, int)
- if value < 1 or value > 6:
- raise MechArmDataException("The range of id is 1 ~ 6, but the received is {}".format(value))
+ if value < 1 or value > 7:
+ raise MechArmDataException("The range of id is 1 ~ 6 or 7, but the received is {}".format(value))
elif parameter == "torque":
torque_min = 150
torque_max = 980
@@ -988,8 +1024,10 @@ def calibration_parameters(**kwargs):
value))
elif parameter == 'end_direction':
check_0_or_1(parameter, value, [1, 2, 3], value_type, MechArmDataException, int)
+ elif parameter == 'is_torque':
+ if value is not None:
+ check_0_or_1(parameter, value, [0, 1], value_type, MyCobot280DataException, int)
elif class_name in ["MyArm", "MyArmSocket"]:
- public_check(parameter_list, kwargs, robot_limit, class_name, MyArmDataException)
for parameter in parameter_list[1:]:
value = kwargs.get(parameter, None)
value_type = type(value)
@@ -1034,7 +1072,7 @@ def calibration_parameters(**kwargs):
elif parameter in ['account', 'password']:
check_value_type(parameter, value_type, MyArmDataException, str)
elif parameter == 'coords':
- check_coords(value, robot_limit, class_name, MyArmDataException)
+ check_coords(parameter, value, robot_limit, class_name, MyArmDataException)
elif parameter in ['rftype', 'move_type', 'end', 'is_linear', 'status', 'mode', 'direction']:
check_0_or_1(parameter, value, [0, 1], value_type, MyArmDataException, int)
elif parameter == 'acceleration':
@@ -1098,13 +1136,13 @@ def calibration_parameters(**kwargs):
for data in value:
data_type = type(data)
check_value_type(parameter, data_type, MyArmDataException, int)
- if data < 0 or data > 3400:
+ if data < 0 or data > 6000:
raise MyArmDataException(
- "The range of speed is 0 ~ 3400, but the received value is {}".format(data))
+ "The range of speed is 0 ~ 6000, but the received value is {}".format(data))
elif parameter in ['servo_id_pdi', 'encode_id']:
check_value_type(parameter, value_type, MyArmDataException, int)
- if value < 1 or value > 7:
- raise MyArmDataException("The range of id is 1 ~ 7, but the received is {}".format(value))
+ if value < 1 or value > 8:
+ raise MyArmDataException("The range of id is 1 ~ 7 or 8, but the received is {}".format(value))
elif parameter == "torque":
torque_min = 150
torque_max = 980
@@ -1126,6 +1164,9 @@ def calibration_parameters(**kwargs):
value_type = type(value)
if parameter == 'id' and value not in robot_limit[class_name][parameter]:
check_id(value, robot_limit[class_name][parameter], MyPalletizer260DataException)
+ elif parameter == 'servo_data_id' and value not in [1, 2, 3, 4, 7]:
+ raise MyPalletizer260DataException(
+ "The id not right, should be in {0}, but received {1}.".format([1, 2, 3, 4, 7], value))
elif parameter == 'rgb':
check_rgb_value(value, MyPalletizer260DataException, class_name)
elif parameter == 'value':
@@ -1154,7 +1195,7 @@ def calibration_parameters(**kwargs):
elif parameter == 'gripper_type':
check_0_or_1(parameter, value, [1, 3, 4], value_type, MyPalletizer260DataException, int)
elif parameter == '_type_1':
- check_0_or_1(parameter, value, [1, 2, 3, 4], value_type, MyPalletizer260DataException, int)
+ check_0_or_1(parameter, value, [1, 2, 3, 4, 5], value_type, MyPalletizer260DataException, int)
# if value not in [0, 1, 10]:
# raise exception_class("The data supported by parameter {} is 0 or 1 or 10, but the received value is {}".format(parameter, value))
elif parameter == 'gripper_value':
@@ -1241,13 +1282,14 @@ def calibration_parameters(**kwargs):
for data in value:
data_type = type(data)
check_value_type(parameter, data_type, MyPalletizer260DataException, int)
- if data < 0 or data > 3400:
+ if data < 0 or data > 6000:
raise MyPalletizer260DataException(
- "The range of speed is 0 ~ 3400, but the received value is {}".format(data))
+ "The range of speed is 0 ~ 6000, but the received value is {}".format(data))
elif parameter in ['servo_id_pdi', 'encode_id']:
check_value_type(parameter, value_type, MyPalletizer260DataException, int)
- if value < 1 or value > 4:
- raise MyPalletizer260DataException("The range of id is 1 ~ 4, but the received is {}".format(value))
+ if value < 1 or (value > 4 and value != 7):
+ raise MyPalletizer260DataException(
+ "The range of id is 1 ~ 4 or 7, but the received is {}".format(value))
elif parameter == "torque":
torque_min = 150
torque_max = 980
@@ -1264,59 +1306,129 @@ def calibration_parameters(**kwargs):
elif parameter == 'end_direction':
check_0_or_1(parameter, value, [1, 2, 3], value_type, MyPalletizer260DataException, int)
- elif class_name in ["MyArmM", "MyArmC"]:
+ elif class_name in ["MyArmM", "MyArmC", "MyArmMControl"]:
class_name = kwargs.pop("class_name", None)
limit_info = robot_limit[class_name]
for parameter in parameter_list[1:]:
value = kwargs[parameter]
value_type = type(value)
- if parameter in ("servo_id", "joint_id") and value not in limit_info[parameter]:
- raise MyArmDataException(
- "The id not right, should be in {0}, but received {1}.".format(limit_info[parameter], value)
- )
+ if parameter in ("servo_id", "joint_id", "coord_id") and value not in limit_info[parameter]:
+ raise ValueError(
+ f"The {parameter} not right, should be in {limit_info[parameter]}, but received {value}.")
elif parameter == 'angle':
i = kwargs['joint_id'] - 1
min_angle = limit_info["angles_min"][i]
max_angle = limit_info["angles_max"][i]
if value < min_angle or value > max_angle:
- raise MyArmDataException(
- f"angle value not right, should be {min_angle} ~ {max_angle}, but received {value}"
- )
+ raise ValueError(f"angle value not right, should be {min_angle} ~ {max_angle}, but received {value}")
elif parameter == 'angles':
+ if not value:
+ raise ValueError("angles value can't be empty")
+
for i, v in enumerate(value):
min_angle = limit_info["angles_min"][i]
max_angle = limit_info["angles_max"][i]
if v < min_angle or v > max_angle:
- raise MyArmDataException(
- f"angle value not right, should be {min_angle} ~ {max_angle}, but received {v}"
- )
+ raise ValueError(
+ f"angle value not right, should be {min_angle} ~ {max_angle}, but received {v}")
+ elif parameter == 'coord':
+ coord_index = kwargs['coord_id'] - 1
+ min_coord = limit_info["coord_min"][coord_index]
+ max_coord = limit_info["coord_max"][coord_index]
+ if not min_coord <= value <= max_coord:
+ raise ValueError(f"coord value not right, should be {min_coord} ~ {max_coord}, but received {value}")
+ elif parameter == 'coords':
+ for i, v in enumerate(value):
+ min_coord = limit_info["coord_min"][i]
+ max_coord = limit_info["coord_max"][i]
+ if not min_coord <= v <= max_coord:
+ raise ValueError(f"coord value not right, should be {min_coord} ~ {max_coord}, but received {v}")
elif parameter == 'encoder':
i = kwargs['servo_id'] - 1
max_encoder = limit_info["encoders_max"][i]
min_encoder = limit_info["encoders_min"][i]
if value < min_encoder or value > max_encoder:
- raise MyArmDataException(
- f"angle value not right, should be {min_encoder} ~ {max_encoder}, but received {value}"
- )
+ raise ValueError(
+ f"angle value not right, should be {min_encoder} ~ {max_encoder}, but received {value}")
elif parameter == 'encoders':
for i, v in enumerate(value):
max_encoder = limit_info["encoders_max"][i]
min_encoder = limit_info["encoders_min"][i]
if v < min_encoder or v > max_encoder:
- raise MyArmDataException(
- f"encoder value not right, should be {min_encoder} ~ {max_encoder}, but received {v}"
- )
+ raise ValueError(
+ f"encoder value not right, should be {min_encoder} ~ {max_encoder}, but received {v}")
elif parameter == "speed":
- check_value_type(parameter, value_type, MyArmDataException, int)
+ check_value_type(parameter, value_type, TypeError, int)
if not 1 <= value <= 100:
- raise MyArmDataException(f"speed value not right, should be 1 ~ 100, the received speed is {value}")
+ raise ValueError(f"speed value not right, should be 1 ~ 100, the received speed is {value}")
elif parameter == "speeds":
assert len(value) == 8, "The length of `speeds` must be 8."
for i, s in enumerate(value):
if not 1 <= s <= 100:
- raise MyArmDataException(
- f"speed value not right, should be 1 ~ 100, the received speed is {value}"
- )
+ raise ValueError(f"speed value not right, should be 1 ~ 100, the received speed is {value}")
elif parameter == "servo_addr":
+ if not isinstance(value, int):
+ raise TypeError(f"The {parameter} must be an integer.")
+
if value in (0, 1, 2, 3, 4):
- raise MyArmDataException("addr 0-4 cannot be modified")
+ if class_name == "MyArmMControl":
+ raise ValueError("modification is not allowed between 0~4, current data id: {}".format(value))
+ raise ValueError("addr 0-4 cannot be modified")
+ elif parameter == "account":
+ pass
+
+ elif parameter == "password":
+ if not re.match(r'^[A-Za-z0-9]{8,63}$', value):
+ raise ValueError("The password must be 8-63 characters long and contain only letters and numbers.")
+ elif parameter == "pin_number":
+ pin = 6
+ if class_name == "MyArmC":
+ pin = 2
+ if not 1 <= value <= pin:
+ raise ValueError(f"The pin number must be between 1 and {pin}.")
+
+ elif parameter in ("direction", "mode", "pin_signal", "is_linear", "move_type", "rftype"):
+ if not isinstance(value, int):
+ raise TypeError(f"The {parameter} must be an integer.")
+ if value not in (0, 1):
+ raise ValueError(f"The {parameter} must be 0 or 1.")
+ elif parameter == "end_direction":
+ if value not in (1, 2, 3):
+ raise ValueError(f"end_direction not right, should be 1 ~ 3, the received end_direction is {value}")
+ elif parameter == "gripper_flag":
+ if value not in (0, 1, 254):
+ raise ValueError(f"gripper_flag not right, should be in (0, 1, 254), the received gripper_flag is {value}")
+ elif parameter == "gripper_value":
+ if not 0 <= value <= 100:
+ raise ValueError(f"gripper_value not right, should be 0 ~ 100, the received gripper_value is {value}")
+ elif parameter == "basic_pin_number":
+ if not isinstance(value, int):
+ raise TypeError(f"The {parameter} must be an integer.")
+
+ if not 1 <= value <= 6:
+ raise ValueError("The basic pin number must be between 1 ~ 6.")
+ elif parameter == "rgb":
+ for v in value:
+ if not 0 <= v <= 255:
+ raise ValueError(f"rgb value not right, should be 0 ~ 255, the received rgb is {value}")
+
+
+def restrict_serial_port(func):
+ """
+ 装饰器，用于限制特定串口号、socket的函数调用。
+ """
+ @functools.wraps(func)
+ def wrapper(self, *args, **kwargs):
+ try:
+ if hasattr(self, '_serial_port'):
+ if self._serial_port.port not in ["/dev/left_arm", "/dev/right_arm"]:
+ raise MercuryRobotException(f"The {func.__name__} function cannot be called. This function is only applicable to the Mercury dual-arm robot.")
+ elif hasattr(self, 'sock'):
+ if not isinstance(self.sock, socket.socket):
+ raise MercuryRobotException(
+ f"The {func.__name__} function cannot be called. The connection must be a valid socket.")
+ return func(self, *args, **kwargs)
+ except MercuryRobotException as e:
+ e = traceback.format_exc()
+ print(f"MercuryRobotException: {e}")
+ return wrapper
diff --git a/pymycobot/generate.py b/pymycobot/generate.py
index 50cb2f8..927d05a 100644
--- a/pymycobot/generate.py
+++ b/pymycobot/generate.py
@@ -343,6 +343,16 @@ def get_encoders(self):
"""
return self._mesg(ProtocolCode.GET_ENCODERS, has_reply=True)
+ def set_encoders_drag(self, encoders, speeds): # TODO 22-5-19 need test
+ """Send all encoders and speeds
+
+ Args:
+ encoders: encoders list.
+ speeds: Obtained by the get_servo_speeds() method
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, encoders=encoders, speeds=speeds)
+ return self._mesg(ProtocolCode.SET_ENCODERS_DRAG, encoders, speeds)
+
# Running status and Settings
def get_joint_min_angle(self, joint_id):
@@ -397,16 +407,16 @@ def set_servo_data(self, servo_id, data_id, value, mode=None):
"""Set the data parameters of the specified address of the steering gear
Args:
- servo_id: Serial number of articulated steering gear. 1 - 6
+ servo_id: Serial number of articulated steering gear. 1 - 7
data_id: Data address.
value: 0 - 4096
mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.
"""
if mode is None:
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id, value=value)
+ self.calibration_parameters(class_name=self.__class__.__name__, servo_data_id=servo_id, address=data_id, value=value)
return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, value)
else:
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id, value=value,
+ self.calibration_parameters(class_name=self.__class__.__name__, servo_data_id=servo_id, address=data_id, value=value,
mode=mode)
return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, [value], mode)
@@ -414,7 +424,7 @@ def get_servo_data(self, servo_id, data_id, mode=None):
"""Read the data parameter of the specified address of the steering gear.
Args:
- servo_id: Serial number of articulated steering gear.1 - 6
+ servo_id: Serial number of articulated steering gear.1 - 7
data_id: Data address.
mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.
@@ -422,11 +432,11 @@ def get_servo_data(self, servo_id, data_id, mode=None):
values 0 - 4096
"""
if mode is not None:
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id, mode=mode)
+ self.calibration_parameters(class_name=self.__class__.__name__, servo_data_id=servo_id, address=data_id, mode=mode)
return self._mesg(
ProtocolCode.GET_SERVO_DATA, servo_id, data_id, mode, has_reply=True
)
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id)
+ self.calibration_parameters(class_name=self.__class__.__name__, servo_data_id=servo_id, address=data_id)
return self._mesg(
ProtocolCode.GET_SERVO_DATA, servo_id, data_id, has_reply=True
)
@@ -474,6 +484,15 @@ def focus_servo(self, servo_id):
"""
self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id)
return self._mesg(ProtocolCode.FOCUS_SERVO, servo_id)
+
+ def focus_all_servos(self):
+ return self._mesg(ProtocolCode.FOCUS_SERVO, 0)
+
+ def focus_all_servos(self):
+ """Power on all servo
+
+ """
+ return self._mesg(ProtocolCode.FOCUS_SERVO, 0)
# Atom IO
def set_color(self, r=0, g=0, b=0):
@@ -507,7 +526,7 @@ def set_gripper_state(self, flag, speed, _type_1=None):
"""Set gripper switch state
Args:
- flag (int): 0 - open, 1 - close, 254 - release
+ flag (int): 0 - open, 1 - close, 10 - release
speed (int): 1 ~ 100
_type_1 (int): default 1
1 : Adaptive gripper. default to adaptive gripper
@@ -553,7 +572,13 @@ def set_basic_output(self, pin_no, pin_signal):
pin_signal: 0 / 1
"""
self.calibration_parameters(class_name=self.__class__.__name__, pin_signal=pin_signal)
- return self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, pin_signal)
+ if (pin_no == 5 and pin_signal == 1):
+ self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, pin_signal)
+ time.sleep(0.5)
+ self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, 0)
+ else:
+ self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, pin_signal)
+ return
def get_basic_input(self, pin_no):
"""Get basic input for M5 version.
@@ -617,4 +642,5 @@ def get_basic_version(self):
return self._mesg(ProtocolCode.GET_BASIC_VERSION, has_reply=True)
+
diff --git a/pymycobot/log.py b/pymycobot/log.py
index 31301cc..bfe4000 100644
--- a/pymycobot/log.py
+++ b/pymycobot/log.py
@@ -5,6 +5,7 @@
def setup_logging(debug=False):
+ # logging.basicConfig()
root_logger = logging.getLogger()
debug_fomatter = logging.Formatter(
@@ -20,8 +21,10 @@ def setup_logging(debug=False):
"python_debug.log", maxBytes=100*1024*1024, backupCount=1)
save.setFormatter(debug_fomatter)
root_logger.addHandler(save)
+ root_logger.setLevel(logging.DEBUG)
else:
logger_handle.setLevel(logging.WARNING)
+ root_logger.setLevel(logging.WARNING)
root_logger.addHandler(logger_handle)
- root_logger.setLevel(0)
+ # root_logger.setLevel(0)
diff --git a/pymycobot/mecharm270.py b/pymycobot/mecharm270.py
index a10271a..9a8c846 100644
--- a/pymycobot/mecharm270.py
+++ b/pymycobot/mecharm270.py
@@ -122,7 +122,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MechArm270, self)._mesg(genre, *args, **kwargs)
if self.thread_lock:
with self.lock:
@@ -131,18 +131,26 @@ def _mesg(self, genre, *args, **kwargs):
return self._res(real_command, has_reply, genre)
def _res(self, real_command, has_reply, genre):
- try_count = 0
- while try_count < 3:
+ if genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
self._write(self._flatten(real_command))
data = self._read(genre)
- if data is not None and data != b'':
- break
- try_count += 1
else:
- return -1
+ try_count = 0
+ while try_count < 3:
+ self._write(self._flatten(real_command))
+ data = self._read(genre)
+ if data is not None and data != b'':
+ break
+ try_count += 1
+ else:
+ return -1
if genre == ProtocolCode.SET_SSID_PWD:
- return None
+ return 1
res = self._process_received(data, genre)
+ if res is None:
+ return None
+ if genre in [ProtocolCode.SET_BASIC_OUTPUT]:
+ return 1
if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [ProtocolCode.GET_BASIC_VERSION,
ProtocolCode.GET_JOINT_MIN_ANGLE,
ProtocolCode.GET_JOINT_MAX_ANGLE,
@@ -179,7 +187,7 @@ def _res(self, real_command, has_reply, genre):
ProtocolCode.SET_FOUR_PIECES_ZERO
]:
return self._process_single(res)
- elif genre in [ProtocolCode.GET_ANGLES]:
+ elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.SOLVE_INV_KINEMATICS]:
return [self._int2angle(angle) for angle in res]
elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
if res:
@@ -197,6 +205,8 @@ def _res(self, real_command, has_reply, genre):
return self._int2coord(res[0])
elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION]:
return self._int2coord(self._process_single(res))
+ elif genre in [ProtocolCode.GET_REBOOT_COUNT]:
+ return self._process_high_low_bytes(res)
elif genre == ProtocolCode.GET_ANGLES_COORDS:
r = []
for index in range(len(res)):
@@ -283,7 +293,7 @@ def sync_send_angles(self, degrees, speed, timeout=15):
Args:
degrees: a list of degree values(List[float]), length 6.
speed: (int) 0 ~ 100
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_angles(degrees, speed)
@@ -301,7 +311,7 @@ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
coords: a list of coord values(List[float])
speed: (int) 0 ~ 100
mode: (int): 0 - angular（default）, 1 - linear
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_coords(coords, speed, mode)
@@ -345,6 +355,29 @@ def jog_rpy(self, end_direction, direction, speed):
self.calibration_parameters(class_name=self.__class__.__name__, end_direction=end_direction, speed=speed)
return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed)
+ def jog_increment_angle(self, joint_id, increment, speed):
+ """ angle step mode
+
+ Args:
+ joint_id: int 1-6.
+ increment: Angle increment value
+ speed: int (0 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, speed=speed)
+ return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
+
+ def jog_increment_coord(self, id, increment, speed):
+ """coord step mode
+
+ Args:
+ id: axis id 1 - 6.
+ increment: Coord increment value
+ speed: int (1 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=id, speed=speed)
+ value = self._coord2int(increment) if id <= 3 else self._angle2int(increment)
+ return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, id, [value], speed)
+
def set_HTS_gripper_torque(self, torque):
"""Set new adaptive gripper torque
@@ -605,6 +638,109 @@ def get_end_type(self):
"""
return self._mesg(ProtocolCode.GET_END_TYPE, has_reply=True)
+ def angles_to_coords(self, angles):
+ """ Convert angles to coordinates
+
+ Args:
+ angles : A float list of all angle.
+
+ Return:
+ list: A float list of all coordinates.
+ """
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True)
+
+ def solve_inv_kinematics(self, target_coords, current_angles):
+ """ Convert target coordinates to angles
+
+ Args:
+ target_coords: A float list of all coordinates.
+ current_angles : A float list of all angle.
+
+ Return:
+ list: A float list of all angle.
+ """
+ angles = [self._angle2int(angle) for angle in current_angles]
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(target_coords[idx]))
+ for angle in target_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
+ def set_vision_mode(self, flag):
+ """Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode.
+ (Only for visual tracking function)
+
+ Args:
+ flag: 0/1; 0 - close; 1 - open
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag)
+ return self._mesg(ProtocolCode.SET_VISION_MODE, flag)
+
+ def is_torque_gripper(self):
+ """Whether it is a force-controlled gripper
+
+ Return:
+ 1 - Force control
+ 0 - Non-force control
+ """
+ return self.get_servo_data(7, 1)
+
+ def set_gripper_state(self, flag, speed, _type_1=None, is_torque=None):
+ """Set gripper switch state
+
+ Args:
+ flag (int): 0 - open, 1 - close, 254 - release
+ speed (int): 1 ~ 100
+ _type_1 (int): default 1
+ 1 : Adaptive gripper. default to adaptive gripper
+ 2 : 5 finger dexterous hand
+ 3 : Parallel gripper, this parameter can be omitted
+ 4 : Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag, speed=speed, _type_1=_type_1, is_torque=is_torque)
+ args = [flag, speed]
+ if _type_1 is not None:
+ args.append(_type_1)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_STATE, *args)
+
+ def set_gripper_value(self, gripper_value, speed, gripper_type=None, is_torque=None):
+ """Set gripper value
+
+ Args:
+ gripper_value (int): 0 ~ 100
+ speed (int): 1 ~ 100
+ gripper_type (int): default 1
+ 1: Adaptive gripper
+ 3: Parallel gripper, this parameter can be omitted
+ 4: Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed,
+ gripper_type=gripper_type, is_torque=is_torque)
+ args = [gripper_value, speed]
+ if gripper_type is not None:
+ args.append(gripper_type)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, *args, has_reply=True)
+
+ def get_reboot_count(self):
+ """Read reboot count
+
+ Return:
+ reboot count
+ """
+ return self._mesg(ProtocolCode.GET_REBOOT_COUNT, has_reply=True)
+
# Other
def wait(self, t):
time.sleep(t)
@@ -615,3 +751,6 @@ def close(self):
def open(self):
self._serial_port.open()
+
+ def go_home(self):
+ return self.send_angles([0, 0, 0, 0, 0, 0], 10)
diff --git a/pymycobot/mecharmsocket.py b/pymycobot/mecharmsocket.py
index 1de2b0d..dc96d92 100644
--- a/pymycobot/mecharmsocket.py
+++ b/pymycobot/mecharmsocket.py
@@ -7,6 +7,7 @@
import math
import socket
import logging
+import threading
from pymycobot.log import setup_logging
from pymycobot.generate import CommandGenerator
@@ -119,25 +120,31 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MechArmSocket, self)._mesg(genre, *args, **kwargs)
# [254,...,255]
# data = self._write(self._flatten(real_command), "socket")
with self.lock:
- try_count = 0
- while try_count < 3:
+ if genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
self._write(self._flatten(real_command), "socket")
data = self._read(genre, method='socket')
- if data is not None and data != b'':
- break
- try_count += 1
else:
- return -1
+ try_count = 0
+ while try_count < 3:
+ self._write(self._flatten(real_command), "socket")
+ data = self._read(genre, method='socket')
+ if data is not None and data != b'':
+ break
+ try_count += 1
+ else:
+ return -1
if genre == ProtocolCode.SET_SSID_PWD:
- return None
+ return 1
res = self._process_received(data, genre)
- if res == []:
+ if res is None:
return None
+ if genre in [ProtocolCode.SET_BASIC_OUTPUT]:
+ return 1
elif res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
ProtocolCode.GET_BASIC_VERSION,
ProtocolCode.GET_JOINT_MIN_ANGLE,
@@ -175,7 +182,7 @@ def _mesg(self, genre, *args, **kwargs):
ProtocolCode.SET_FOUR_PIECES_ZERO
]:
return self._process_single(res)
- elif genre in [ProtocolCode.GET_ANGLES]:
+ elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.SOLVE_INV_KINEMATICS]:
return [self._int2angle(angle) for angle in res]
elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
if res:
@@ -194,6 +201,8 @@ def _mesg(self, genre, *args, **kwargs):
elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION,
ProtocolCode.GET_ATOM_VERSION]:
return self._int2coord(self._process_single(res))
+ elif genre in [ProtocolCode.GET_REBOOT_COUNT]:
+ return self._process_high_low_bytes(res)
elif genre == ProtocolCode.GET_ANGLES_COORDS:
r = []
for index in range(len(res)):
@@ -281,16 +290,16 @@ def sync_send_angles(self, degrees, speed, timeout=15):
Args:
degrees: a list of degree values(List[float]), length 6.
speed: (int) 0 ~ 100
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_angles(degrees, speed)
while time.time() - t < timeout:
f = self.is_in_position(degrees, 0)
if f == 1:
- break
+ return 1
time.sleep(0.1)
- return 1
+ return 0
def sync_send_coords(self, coords, speed, mode=0, timeout=15):
"""Send the coord in synchronous state and return when the target point is reached
@@ -299,7 +308,7 @@ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
coords: a list of coord values(List[float])
speed: (int) 0 ~ 100
mode: (int): 0 - angular（default）, 1 - linear
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_coords(coords, speed, mode)
@@ -325,6 +334,29 @@ def jog_rpy(self, end_direction, direction, speed):
self.calibration_parameters(class_name=self.__class__.__name__, end_direction=end_direction, speed=speed)
return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed)
+ def jog_increment_angle(self, joint_id, increment, speed):
+ """ angle step mode
+
+ Args:
+ joint_id: int 1-6.
+ increment: Angle increment value
+ speed: int (0 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, speed=speed)
+ return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
+
+ def jog_increment_coord(self, id, increment, speed):
+ """coord step mode
+
+ Args:
+ id: axis id 1 - 6.
+ increment: Coord increment value
+ speed: int (1 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=id, speed=speed)
+ value = self._coord2int(increment) if id <= 3 else self._angle2int(increment)
+ return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, id, [value], speed)
+
def set_HTS_gripper_torque(self, torque):
"""Set new adaptive gripper torque
@@ -622,6 +654,109 @@ def get_gpio_in(self, pin_no):
"""
return self._mesg(ProtocolCode.GET_GPIO_IN, pin_no, has_reply=True)
+ def angles_to_coords(self, angles):
+ """ Convert angles to coordinates
+
+ Args:
+ angles : A float list of all angle.
+
+ Return:
+ list: A float list of all coordinates.
+ """
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True)
+
+ def solve_inv_kinematics(self, target_coords, current_angles):
+ """ Convert target coordinates to angles
+
+ Args:
+ target_coords: A float list of all coordinates.
+ current_angles : A float list of all angle.
+
+ Return:
+ list: A float list of all angle.
+ """
+ angles = [self._angle2int(angle) for angle in current_angles]
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(target_coords[idx]))
+ for angle in target_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
+ def set_vision_mode(self, flag):
+ """Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode.
+ (Only for visual tracking function)
+
+ Args:
+ flag: 0/1; 0 - close; 1 - open
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag)
+ return self._mesg(ProtocolCode.SET_VISION_MODE, flag)
+
+ def is_torque_gripper(self):
+ """Whether it is a force-controlled gripper
+
+ Return:
+ 1 - Force control
+ 0 - Non-force control
+ """
+ return self.get_servo_data(7, 1)
+
+ def set_gripper_state(self, flag, speed, _type_1=None, is_torque=None):
+ """Set gripper switch state
+
+ Args:
+ flag (int): 0 - open, 1 - close, 254 - release
+ speed (int): 1 ~ 100
+ _type_1 (int): default 1
+ 1 : Adaptive gripper. default to adaptive gripper
+ 2 : 5 finger dexterous hand
+ 3 : Parallel gripper, this parameter can be omitted
+ 4 : Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag, speed=speed, _type_1=_type_1, is_torque=is_torque)
+ args = [flag, speed]
+ if _type_1 is not None:
+ args.append(_type_1)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_STATE, *args)
+
+ def set_gripper_value(self, gripper_value, speed, gripper_type=None, is_torque=None):
+ """Set gripper value
+
+ Args:
+ gripper_value (int): 0 ~ 100
+ speed (int): 1 ~ 100
+ gripper_type (int): default 1
+ 1: Adaptive gripper
+ 3: Parallel gripper, this parameter can be omitted
+ 4: Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed,
+ gripper_type=gripper_type, is_torque=is_torque)
+ args = [gripper_value, speed]
+ if gripper_type is not None:
+ args.append(gripper_type)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, *args, has_reply=True)
+
+ def get_reboot_count(self):
+ """Read reboot count
+
+ Return:
+ reboot count
+ """
+ return self._mesg(ProtocolCode.GET_REBOOT_COUNT, has_reply=True)
+
# Other
def wait(self, t):
time.sleep(t)
@@ -629,3 +764,6 @@ def wait(self, t):
def close(self):
self.sock.close()
+
+ def go_home(self):
+ self.sync_send_angles([0, 0, 0, 0, 0, 0], 30)
diff --git a/pymycobot/mercury.py b/pymycobot/mercury.py
index 89f38cd..2b7ed84 100644
--- a/pymycobot/mercury.py
+++ b/pymycobot/mercury.py
@@ -2,13 +2,13 @@
import time
import threading
+import serial
+import serial.serialutil
from pymycobot.mercury_api import MercuryCommandGenerator
-from pymycobot.error import calibration_parameters
-
class Mercury(MercuryCommandGenerator):
- def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
+ def __init__(self, port="/dev/ttyAMA1", baudrate="115200", timeout=0.1, debug=False, save_serial_log=False):
"""
Args:
port : port string
@@ -17,9 +17,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
debug : whether show debug info
"""
super(Mercury, self).__init__(debug)
- self.calibration_parameters = calibration_parameters
- import serial
-
+ self.save_serial_log = save_serial_log
self._serial_port = serial.Serial()
self._serial_port.port = port
self._serial_port.baudrate = baudrate
@@ -27,10 +25,22 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
self._serial_port.rts = False
self._serial_port.open()
self.lock = threading.Lock()
- self.has_reply_command = []
- self.is_stop = False
- self.read_threading = threading.Thread(target=self.read_thread, daemon=True)
+ self.lock_out = threading.Lock()
+ self.read_threading = threading.Thread(target=self.read_thread)
+ self.read_threading.daemon = True
self.read_threading.start()
+ try:
+ self._serial_port.write(b"\xfa")
+ except serial.serialutil.SerialException as e:
+ self._serial_port.close()
+ time.sleep(0.5)
+ self._serial_port = serial.Serial()
+ self._serial_port.port = port
+ self._serial_port.baudrate = baudrate
+ self._serial_port.timeout = timeout
+ self._serial_port.rts = False
+ self._serial_port.open()
+ self.get_limit_switch()
def open(self):
self._serial_port.open()
diff --git a/pymycobot/mercury_api.py b/pymycobot/mercury_api.py
index 888bdfe..c1ab51a 100644
--- a/pymycobot/mercury_api.py
+++ b/pymycobot/mercury_api.py
@@ -1,19 +1,100 @@
-
# coding=utf-8
-import sys
-import logging
-import time
-import struct
+import locale
+import numpy as np
-from pymycobot.log import setup_logging
-from pymycobot.error import calibration_parameters
+from pymycobot.error import restrict_serial_port
+from pymycobot.common import ProtocolCode, FingerGripper
+from pymycobot.robot_info import _interpret_status_code
from pymycobot.close_loop import CloseLoop
-from pymycobot.common import ProtocolCode, write, read
class MercuryCommandGenerator(CloseLoop):
-
+ def __init__(self, debug=False):
+ super(MercuryCommandGenerator, self).__init__(debug)
+ # 同步模式
+ self.language, _ = locale.getdefaultlocale()
+ if self.language not in ["zh_CN", "en_US"]:
+ self.language = "en_US"
+ self.max_joint, self.min_joint = 0, 0
+
+ def _joint_limit_init(self):
+ max_joint = np.zeros(7)
+ min_joint = np.zeros(7)
+ for i in range(7):
+ max_joint[i] = self.get_joint_max_angle(i + 1)
+ min_joint[i] = self.get_joint_min_angle(i + 1)
+ return max_joint, min_joint
+
+ def _joint_limit_judge(self, angles):
+ offset = 3
+ try:
+ for i in range(7):
+ if self.min_joint[i] + offset < angles[i] < self.max_joint[i] - offset:
+ pass
+ else:
+ if self.language == "zh_CN":
+ return f"当前角度为{angles[i]}, 角度范围为： {self.min_joint[i]} ~ {self.max_joint[i]}"
+ return f"current value = {angles[i]}, limit is {self.min_joint[i]} ~ {self.max_joint[i]}"
+ except TypeError:
+ return "joint limit error"
+ return "over limit error {}".format(angles)
+
+ def _Singularity(self, angles):
+ try:
+ # Joint 6: 0 and 180 degrees are singular points
+ singular_angles = [0, 180]
+ state = ""
+ offset = 5
+ for singular in singular_angles:
+ if singular - offset < angles[5] < singular + offset:
+ if self.language == "zh_CN":
+ return f"在关节 6 处检测到奇点：{angles[5]} 度"
+ return f"Singularity detected at joint 6: {angles[5]} degrees"
+ return state
+ except:
+ return "Singularity error"
+
+ def _check_coords(self, new_coords, is_print=0):
+ try:
+ first_three = new_coords[:3]
+ first_three[2] -= 83.64
+ info = ""
+ # Calculate the Euclidean norm (magnitude)
+ magnitude = np.linalg.norm(first_three)
+ if is_print == 1:
+ if self.language == "zh_CN":
+ info += f"当前臂展为{magnitude}, 最大的臂展为{self.arm_span}"
+ else:
+ info += f"Arm span is {magnitude}, max is {self.arm_span}"
+
+ # if magnitude > self.arm_span - 10:
+ # if self.language == "zh_CN":
+ # info += f"当前臂展为{magnitude}超出物理限位, 最大的臂展为{self.arm_span}"
+ # else:
+ # info += f"Arm span is {magnitude} exceeds physical limit, max is {self.arm_span}"
+ return info
+ except:
+ return "check coords error"
+
+ def _status_explain(self, status):
+ error_info = _interpret_status_code(self.language, status)
+ if error_info != "":
+ self.arm_span = 440
+ if 0x00 < status <= 0x07:
+ angles = self.get_angles()
+ if type(self.max_joint) == int and self.max_joint == 0:
+ self.max_joint, self.min_joint = self._joint_limit_init()
+ error_info += self._joint_limit_judge(angles)
+ elif status in [32, 33]:
+ error_coords = self.get_coords()
+ error_info += self._check_coords(error_coords, 1)
+ elif status == 36:
+ angles = self.get_angles()
+ error_info += self._Singularity(angles)
+
+ return error_info
+
def _mesg(self, genre, *args, **kwargs):
"""
@@ -26,92 +107,203 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- read_data = super(MercuryCommandGenerator, self)._mesg(genre, *args, **kwargs)
- if read_data is None:
+ return_data = super(MercuryCommandGenerator, self)._mesg(
+ genre, *args, **kwargs)
+ if isinstance(return_data, tuple):
+ valid_data, data_len = return_data
+ elif isinstance(return_data, int):
+ return return_data
+ else:
return None
- elif read_data == 1:
- return 1
- valid_data, data_len = read_data
res = []
- if data_len in [6, 8, 12, 14, 16, 24, 26, 60]:
+ hand_address = None
+ if genre == ProtocolCode.MERCURY_GET_TOQUE_GRIPPER:
+ if len(valid_data) > 1:
+ hand_address = self._decode_int16(valid_data[1:3])
+ valid_data = valid_data[3:]
+ data_len -= 3
+ else:
+ return valid_data[0]
+ # print(data_len, valid_data)
+ if data_len in [6, 8, 12, 14, 16, 20, 24, 26, 60]:
if data_len == 8 and (genre == ProtocolCode.IS_INIT_CALIBRATION):
if valid_data[0] == 1:
return 1
n = len(valid_data)
- for v in range(1,n):
+ for v in range(1, n):
res.append(valid_data[v])
elif data_len == 8 and genre == ProtocolCode.GET_DOWN_ENCODERS:
- res = self.bytes4_to_int(valid_data)
- elif data_len == 6 and genre in [ProtocolCode.GET_SERVO_STATUS, ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.GET_SERVO_CURRENTS]:
+ i = 0
+ while i < data_len:
+ byte_value = int.from_bytes(
+ valid_data[i:i + 4], byteorder='big', signed=True)
+ i += 4
+ res.append(byte_value)
+ elif data_len == 6 and genre in [ProtocolCode.GET_SERVO_STATUS, ProtocolCode.GET_SERVO_VOLTAGES,
+ ProtocolCode.GET_SERVO_CURRENTS]:
for i in range(data_len):
res.append(valid_data[i])
else:
+
for header_i in range(0, len(valid_data), 2):
- one = valid_data[header_i : header_i + 2]
+ one = valid_data[header_i: header_i + 2]
res.append(self._decode_int16(one))
elif data_len == 2:
if genre in [ProtocolCode.IS_SERVO_ENABLE]:
- return [self._decode_int8(valid_data[1:2])]
+ return self._decode_int8(valid_data[1:2])
+ elif genre in [ProtocolCode.GET_LIMIT_SWITCH]:
+ for i in valid_data:
+ res.append(i)
elif genre in [ProtocolCode.GET_ERROR_INFO]:
- return [self._decode_int8(valid_data[1:])]
- res.append(self._decode_int16(valid_data))
+ res.append(self._decode_int8(valid_data[1:]))
+ else:
+ res.append(self._decode_int16(valid_data))
+ if hand_address in [FingerGripper.GET_HAND_MAJOR_FIRMWARE_VERSION]:
+ res[0] /=10
elif data_len == 3:
- res.append(self._decode_int16(valid_data[1:]))
+ if genre in [ProtocolCode.GET_DIGITAL_INPUTS]:
+ for i in valid_data:
+ res.append(i)
+ else:
+ res.append(self._decode_int16(valid_data[1:]))
elif data_len == 4:
- if genre == ProtocolCode.COBOTX_GET_ANGLE:
- res = self.bytes4_to_int(valid_data)
- for i in range(1,4):
- res.append(valid_data[i])
+ if genre in [ProtocolCode.COBOTX_GET_ANGLE, ProtocolCode.GET_ENCODER, ProtocolCode.GET_DYNAMIC_PARAMETERS]:
+ byte_value = int.from_bytes(
+ valid_data, byteorder='big', signed=True)
+ if genre in [ProtocolCode.GET_DYNAMIC_PARAMETERS]:
+ byte_value /= 1000
+ res.append(byte_value)
+ else:
+ for i in range(1, 4):
+ res.append(valid_data[i])
elif data_len == 7:
- error_list = [i for i in valid_data]
- for i in error_list:
- if i in range(16,23):
- res.append(1)
- elif i in range(23,29):
- res.append(2)
- elif i in range(32,112):
- res.append(3)
- else:
- res.append(i)
- elif data_len == 28:
- res = self.bytes4_to_int(valid_data)
- elif data_len == 40:
+ # if genre ==ProtocolCode.GET_TORQUE_COMP:
+ for i in range(0, data_len):
+ res.append(valid_data[i])
+ # else:
+ # error_list = [i for i in valid_data]
+ # for i in error_list:
+ # if i in range(16, 23):
+ # res.append(1)
+ # elif i in range(23, 29):
+ # res.append(2)
+ # elif i in range(32, 112):
+ # res.append(3)
+ # else:
+ # res.append(i)
+ elif data_len in [28, 32]: # 28 left get_zero_pos, 32 right get_zero_pos
+ for i in range(0, data_len, 4):
+ byte_value = int.from_bytes(
+ valid_data[i:i + 4], byteorder='big', signed=True)
+ res.append(byte_value)
+ elif data_len == 40 and genre == ProtocolCode.GET_ANGLES_COORDS:
i = 0
while i < data_len:
if i < 28:
- res += self.bytes4_to_int(valid_data)
- i+=4
+ byte_value = int.from_bytes(
+ valid_data[i:i + 4], byteorder='big', signed=True)
+ res.append(byte_value)
+ i += 4
else:
- one = valid_data[i : i + 2]
+ one = valid_data[i: i + 2]
res.append(self._decode_int16(one))
- i+=2
- elif data_len == 30:
+ i += 2
+ elif data_len == 40 and genre == ProtocolCode.MERCURY_ROBOT_STATUS:
+ # 右臂上位机错误
i = 0
res = []
- while i < 30:
- if i < 9 or i >= 23:
+ while i < data_len:
+ if i < 10 or i >= 30:
+ res.append(valid_data[i])
+ i += 1
+ elif i < 30:
+ one = valid_data[i: i + 2]
+ res.append(self._decode_int16(one))
+ i += 2
+ elif data_len == 41 and genre == ProtocolCode.MERCURY_ROBOT_STATUS:
+ # 图灵右臂上位机错误
+ i = 0
+ res = []
+ while i < data_len:
+ if i < 9:
+ res.append(valid_data[i])
+ i += 1
+ else:
+ one = valid_data[i: i + 2]
+ res.append(self._decode_int16(one))
+ i += 2
+ elif data_len in [37, 46]:
+ # X1 left status
+ if data_len == 37:
+ add_index = 9
+ else:
+ add_index = 10
+ i = 0
+ res = []
+ while i < data_len:
+ if i < add_index:
res.append(valid_data[i])
- i+=1
- elif i < 23:
- one = valid_data[i : i + 2]
+ i += 1
+ else:
+ one = valid_data[i: i + 2]
res.append(self._decode_int16(one))
- i+=2
+ i += 2
elif data_len == 38:
i = 0
res = []
while i < data_len:
if i < 10 or i >= 30:
res.append(valid_data[i])
- i+=1
+ i += 1
elif i < 38:
- one = valid_data[i : i + 2]
+ one = valid_data[i: i + 2]
res.append(self._decode_int16(one))
- i+=2
+ i += 2
elif data_len == 56:
- for i in range(0, data_len, 8):
-
- byte_value = int.from_bytes(valid_data[i:i+4], byteorder='big', signed=True)
- res.append(byte_value)
+ i = 0
+ while i < data_len:
+ byte_value_send = int.from_bytes(
+ valid_data[i:i + 4], byteorder='big', signed=True)
+ i += 4
+ byte_value_current = int.from_bytes(
+ valid_data[i:i + 4], byteorder='big', signed=True)
+ res.append([byte_value_send, byte_value_current])
+ i += 4
+ elif data_len == 48:
+ i = 0
+ j = 0
+ res = [0, 0]
+ angles = []
+ coords = []
+ while i < data_len:
+ if i < 28:
+ byte_value_send = int.from_bytes(
+ valid_data[i:i + 4], byteorder='big', signed=True)
+ angles.append(self._int2angle(byte_value_send))
+ i += 4
+ elif i < 40:
+
+ one = valid_data[i: i + 2]
+ one = self._decode_int16(one)
+ if j < 3:
+ one = self._int2coord(one)
+ else:
+ one = self._int2angle(one)
+ coords.append(one)
+ j += 1
+ i += 2
+ else:
+ res.append(valid_data[i])
+ i += 1
+ res[0] = angles
+ res[1] = coords
+ res[2] = 0
+ res[3] = 0
+ # elif genre == ProtocolCode.MERCURY_GET_TOQUE_GRIPPER:
+ # res = self._decode_int16(valid_data[-2:])
+ # address = self._decode_int16(valid_data[1:3])
+ # if address == 1:
+ # res /= 10
else:
if genre in [
ProtocolCode.GET_SERVO_VOLTAGES,
@@ -119,12 +311,11 @@ def _mesg(self, genre, *args, **kwargs):
ProtocolCode.GET_SERVO_TEMPS,
]:
for i in range(data_len):
- data1 = self._decode_int8(valid_data[i : i + 1])
+ data1 = self._decode_int8(valid_data[i: i + 1])
res.append(0xFF & data1 if data1 < 0 else data1)
res.append(self._decode_int8(valid_data))
if res == []:
return None
-
if genre in [
ProtocolCode.ROBOT_VERSION,
ProtocolCode.GET_ROBOT_ID,
@@ -149,7 +340,7 @@ def _mesg(self, genre, *args, **kwargs):
ProtocolCode.GET_FRESH_MODE,
ProtocolCode.GET_GRIPPER_MODE,
ProtocolCode.SET_SSID_PWD,
- ProtocolCode.GET_ERROR_DETECT_MODE,
+ # ProtocolCode.GET_ERROR_DETECT_MODE,
ProtocolCode.POWER_ON,
ProtocolCode.POWER_OFF,
ProtocolCode.RELEASE_ALL_SERVOS,
@@ -161,10 +352,16 @@ def _mesg(self, genre, *args, **kwargs):
ProtocolCode.IS_BTN_CLICKED,
ProtocolCode.GET_CONTROL_MODE,
ProtocolCode.GET_VR_MODE,
- ProtocolCode.GET_FILTER_LEN
+ ProtocolCode.GET_FILTER_LEN,
+ ProtocolCode.MERCURY_ERROR_COUNTS,
+ ProtocolCode.GET_MAX_ACC,
+ ProtocolCode.GET_MONITOR_MODE,
+ ProtocolCode.GET_COLLISION_MODE,
+ ProtocolCode.GET_DYNAMIC_PARAMETERS,
+ ProtocolCode.GET_ERROR_INFO
]:
return self._process_single(res)
- elif genre in [ProtocolCode.GET_ANGLES]:
+ elif genre in [ProtocolCode.GET_DRAG_FIFO]:
return [self._int2angle(angle) for angle in res]
elif genre in [
ProtocolCode.GET_COORDS,
@@ -210,35 +407,230 @@ def _mesg(self, genre, *args, **kwargs):
if res[i] == 1:
r.append(i)
return r
- elif genre in [ProtocolCode.COBOTX_GET_ANGLE, ProtocolCode.COBOTX_GET_SOLUTION_ANGLES, ProtocolCode.GET_POS_OVER]:
- return self._int2angle(res[0])
+ elif genre in [ProtocolCode.COBOTX_GET_SOLUTION_ANGLES, ProtocolCode.MERCURY_GET_POS_OVER_SHOOT,
+ ProtocolCode.GET_SERVO_CW]:
+ return self._int2angle(res[0])
+ elif genre == ProtocolCode.GET_ANGLES:
+ return [self._int3angle(angle) for angle in res]
+ elif genre == ProtocolCode.SOLVE_INV_KINEMATICS:
+ return [self._int2angle(angle) for angle in res]
+ elif genre == ProtocolCode.COBOTX_GET_ANGLE:
+ return self._int2angle(res[0])
elif genre == ProtocolCode.MERCURY_ROBOT_STATUS:
if len(res) == 23:
- i = 9
- for i in range(9, len(res)):
- if res[i] != 0:
- data = bin(res[i])[2:]
- res[i] = []
- while len(data) != 16:
- data = "0"+data
- for j in range(16):
- if data[j] != "0":
- res[i].append(15-j)
- return res
+ index = 9
else:
- for i in range(10, len(res)):
- if res[i] != 0:
- data = bin(res[i])[2:]
- res[i] = []
- while len(data) != 16:
- data = "0"+data
- for j in range(16):
- if data[j] != "0":
- res[i].append(15-j)
- return res
+ index = 10
+ for i in range(index, len(res)):
+ if res[i] != 0:
+ data = bin(res[i])[2:]
+ res[i] = []
+ while len(data) != 16:
+ data = "0" + data
+ for j in range(16):
+ if data[j] != "0":
+ error_id = 15 - j
+ res[i].append(error_id)
+ if res[i] == []:
+ res[i] = 0
+ return res
+ elif genre == ProtocolCode.GET_MOTORS_RUN_ERR:
+ for i in range(len(res)):
+ if res[i] != 0:
+ data = bin(res[i])[2:]
+ res[i] = []
+ while len(data) != 16:
+ data = "0" + data
+ for j in range(16):
+ if data[j] != "0":
+ error_id = 15 - j
+ res[i].append(error_id)
+ if res[i] == []:
+ res[i] = 0
+ return res
else:
+ if isinstance(res, list) and len(res) == 1:
+ return res[0]
return res
-
+
+ @restrict_serial_port
+ def get_base_coords(self):
+ """get base coords"""
+ return self._mesg(ProtocolCode.MERCURY_GET_BASE_COORDS)
+
+ @restrict_serial_port
+ def send_base_coord(self, coord_id, base_coord, speed, _async=False):
+ """Single coordinate control with the torso base as the coordinate system
+
+ Args:
+ coord_id (int): 1 to 6 correspond to x, y, z, rx, ry, rz
+ base_coord (float): coord value.
+ The coord range of `X` is -351.11 ~ 566.92.
+ The coord range of `Y` is -645.91 ~ 272.12.
+ The coord range of `Y` is -262.91 ~ 655.13.
+ The coord range of `RX` is -180 ~ 180.
+ The coord range of `RY` is -180 ~ 180.
+ The coord range of `RZ` is -180 ~ 180.
+ speed (int): 1 ~ 100
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coord_id=coord_id, base_coord=base_coord, speed=speed, serial_port=self._serial_port.port)
+ if coord_id < 4:
+ coord = self._coord2int(base_coord)
+ else:
+ coord = self._angle2int(base_coord)
+ return self._mesg(ProtocolCode.MERCURY_SET_BASE_COORD, coord_id, [coord], speed, _async=_async, has_reply=True)
+
+ @restrict_serial_port
+ def send_base_coords(self, base_coords, speed, _async=False):
+ """Full coordinate control
+
+ Args:
+ base_coords (list): coordinate value, [x, y, z, rx, ry, rz]
+ speed (int): 1 ~ 100
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, base_coords=base_coords, speed=speed, serial_port=self._serial_port.port)
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(base_coords[idx]))
+ for angle in base_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.MERCURY_SET_BASE_COORDS, coord_list, speed, _async=_async, has_reply=True)
+
+ @restrict_serial_port
+ def jog_base_coord(self, axis, direction, speed, _async=True):
+ """Single-coordinate unidirectional motion control
+
+ Args:
+ axis (int): 1 ~ 6 correspond to [x, y, z, rx, ry, rz]
+ direction (int): Direction of movement. 0 or 1.
+ speed (int): 1 ~ 100.
+ sync (bool): Whether to wait for the movement to complete. Default to True.
+
+ Return:
+ 0: End of exercise.
+ 1 ~ 7: Corresponding joint exceeds limit.
+ 16 ~ 19: Collision protection.
+ 32 ~ 35: Coordinates have no adjacent solutions.
+ 49: Not powered on.
+ 50: Motor abnormality.
+ 51: Motor encoder error
+ 52: Not reaching the designated location or not reaching the designated location for more than 5 minutes (only J11, J12 available)
+ """
+ return self._mesg(ProtocolCode.MERCURY_JOG_BASE_COORD, axis, direction, speed, _async=_async, has_reply=True)
+
+ @restrict_serial_port
+ def set_servo_cw(self, head_id, err_angle):
+ """Set the joint in-place feedback error angle
+
+ Args:
+ head_id (int): Joint ID, 11 or 12.
+ err_angle (float): Error range is 0 ~ 5.
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, head_id=head_id, err_angle=err_angle)
+ return self._mesg(ProtocolCode.SET_SERVO_CW, head_id, [self._angle2int(err_angle)])
+
+ @restrict_serial_port
+ def get_servo_cw(self, head_id):
+ """Get the joint in-place feedback error angle
+
+ Args:
+ head_id (int): Joint ID, 11 or 12.
+
+ Returns:
+ float: Error angle, range is 0 ~ 5.
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, head_id=head_id)
+ return self._mesg(ProtocolCode.GET_SERVO_CW, head_id)
+
+ @restrict_serial_port
+ def clear_waist_queue(self):
+ """Clear the cache points of the three motors in the torso
+ """
+ return self._mesg(ProtocolCode.CLEAR_WAIST_QUEUE)
+
+ @restrict_serial_port
+ def jog_base_increment_coord(self, axis_id, increment, speed, _async=True):
+ """Single coordinate incremental motion control
+
+ Args:
+ axis_id (int): axis id, range 1 ~ 6 corresponds to [x,y,z,rx,ry,rz]
+ increment (float): Incremental value
+ speed (int): speed
+
+ Return:
+ 0: End of exercise.
+ 1 ~ 7: Corresponding joint exceeds limit.
+ 16 ~ 19: Collision protection.
+ 32 ~ 35: Coordinates have no adjacent solutions.
+ 49: Not powered on.
+ 50: Motor abnormality.
+ 51: Motor encoder error
+ 52: Not reaching the designated location or not reaching the designated location for more than 5 minutes (only J11, J12 available)
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, coord_id=axis_id, speed=speed)
+ coord_list = []
+ if axis_id < 4:
+ coord_list.append(self._coord2int(increment))
+ else:
+ coord_list.append(self._angle2int(increment))
+ return self._mesg(ProtocolCode.JOG_BASE_INCREMENT_COORD, axis_id, coord_list, speed, has_reply=True,
+ _async=_async)
+ def is_in_position(self, data, mode=0):
+ """Judge whether in the position.
+
+ Args:
+ data: A data list, angles or coords. angles len 6, coords len 6.
+ mode: 0 - angles, 1 - coords, 2 - base coords
+
+ Return:
+ 1 - True\n
+ 0 - False\n
+ -1 - Error
+ """
+ if mode in [1,2]:
+ if mode == 2:
+ self.calibration_parameters(class_name=self.__class__.__name__, base_coords=data, serial_port=self._serial_port.port)
+ else:
+ self.calibration_parameters(class_name=self.__class__.__name__, coords=data)
+ data_list = []
+ for idx in range(3):
+ data_list.append(self._coord2int(data[idx]))
+ for idx in range(3, 6):
+ data_list.append(self._angle2int(data[idx]))
+ elif mode == 0:
+ self.calibration_parameters(class_name=self.__class__.__name__, angles=data)
+ data_list = [self._angle2int(i) for i in data]
+ else:
+ raise Exception("mode is not right, please input 0 or 1 or 2")
+ return self._mesg(ProtocolCode.IS_IN_POSITION, data_list, mode)
-
\ No newline at end of file
+ def write_waist_sync(self, current_angle, target_angle, speed):
+ """_summary_
+
+ Args:
+ current_angle (_type_): _description_
+ target_angle (_type_): _description_
+ speed (_type_): _description_
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__,
+ current_angle=current_angle, target_angle=target_angle, speed=speed)
+ return self._mesg(ProtocolCode.WRITE_WAIST_SYNC, [self._angle2int(current_angle)], [self._angle2int(target_angle)], speed)
+
+ @restrict_serial_port
+ def jog_base_rpy(self, axis, direction, speed, _async=True):
+ """Rotate the end point around the fixed axis of the base coordinate system
+
+ Args:
+ axis (int): 1 ~ 3. 1 - Roll, 2 - Pitch, 3 - Yaw
+ direction (int): 1 - Forward. 0 - Reverse.
+ speed (int): 1 ~ 100.
+ """
+ self.calibration_parameters(
+ class_name=self.__class__.__name__, axis=axis, direction=direction, speed=speed)
+ return self._mesg(ProtocolCode.JOG_BASE_RPY, axis, direction, speed, _async=_async, has_reply=True)
+
diff --git a/pymycobot/mercury_arms_socket.py b/pymycobot/mercury_arms_socket.py
new file mode 100644
index 0000000..43b28d9
--- /dev/null
+++ b/pymycobot/mercury_arms_socket.py
@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+import json
+import struct
+import socket
+import threading
+from pymycobot.mercury_api import MercuryCommandGenerator
+from pymycobot.error import calibration_parameters
+
+"""
+for mercury x1 robot arms (six-axis or seven-axis), using socket to communicate with the robot
+"""
+
+
+class MercuryArmsSocket(MercuryCommandGenerator):
+ def __init__(self, arm, ip, netport=9000, debug=False):
+ """
+ Arm socket connection
+ Args:
+ arm: 'left_arm' or 'right_arm'
+ ip: ip address
+ netport: port
+ debug: debug mode
+ """
+ super(MercuryArmsSocket, self).__init__(debug)
+ self.arm = arm
+ self.calibration_parameters = calibration_parameters
+ self.SERVER_IP = ip
+ self.SERVER_PORT = netport
+ self.sock = self.connect_socket()
+ self.lock = threading.Lock()
+ self.lock_out = threading.Lock()
+ self.read_threading = threading.Thread(target=self.read_thread, args=("socket", ))
+ self.read_threading.daemon = True
+ self.read_threading.start()
+ self.get_limit_switch()
+
+ def connect_socket(self):
+ sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ sock.connect((self.SERVER_IP, self.SERVER_PORT))
+ return sock
+
+ def open(self):
+ self.sock = self.connect_socket()
+
+ def close(self):
+ self.sock.close()
+ self.sock = None
+
+ def __format(self, command: str) -> bytes:
+ send_data = {"command": command, "arm": self.arm}
+ data_byter = json.dumps(send_data).encode('utf-8')
+ date_length = struct.pack('!I', len(data_byter))
+ return b''.join([date_length, data_byter])
+
+ def _write(self, command, method=None):
+ log_command = " ".join(map(lambda n: hex(n)[2:], command))
+ self.log.debug("_write: {}".format(log_command))
+ self.sock.sendall(self.__format(command))
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/pymycobot/mercury_ros_api.py b/pymycobot/mercury_ros_api.py
new file mode 100644
index 0000000..b5aa336
--- /dev/null
+++ b/pymycobot/mercury_ros_api.py
@@ -0,0 +1,524 @@
+#!/usr/bin/env python
+# coding=UTF-8
+import time
+import threading
+import time
+import sys
+import subprocess
+import os
+import glob
+
+try:
+ with open('/proc/device-tree/model', 'r') as model_file:
+ model = model_file.read().strip().lower() # nvidia jetson xavier nx developer kit
+ if "xavier" in model:
+ import rospy
+ import actionlib
+ import tf2_ros
+ import tf_conversions
+ from std_msgs.msg import Float32
+ from actionlib_msgs.msg import *
+ from actionlib_msgs.msg import GoalID
+ from actionlib_msgs.msg import GoalStatusArray
+ from tf.transformations import quaternion_from_euler
+ from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+ from geometry_msgs.msg import Point
+ from geometry_msgs.msg import Point
+ from geometry_msgs.msg import Twist
+ from geometry_msgs.msg import PoseWithCovarianceStamped
+ from geometry_msgs.msg import PoseStamped, PoseArray
+except Exception as e:
+ pass
+
+# MapNavigation
+
+
+class MapNavigation:
+ def __init__(self):
+ # Initialize the ROS node
+ rospy.init_node('map_navigation', anonymous=False)
+
+ # ros publisher
+ self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
+ self.pub_setpose = rospy.Publisher(
+ '/initialpose', PoseWithCovarianceStamped, queue_size=10)
+ self.pub_cancel = rospy.Publisher(
+ '/move_base/cancel', GoalID, queue_size=10)
+ self.pub_tempPose = rospy.Publisher(
+ 'move_base_simple/goal_temp', PoseStamped, queue_size=1)
+ self.goal_pub = rospy.Publisher(
+ 'move_base_simple/goal', PoseStamped, queue_size=1)
+
+ # ros subscriber
+ # self.voltage_subscriber = rospy.Subscriber("/PowerVoltage", Float32, self.voltage_callback) #Create a battery-voltage topic subscriber
+ self.initialpose_subscriber = rospy.Subscriber(
+ '/initialpose', PoseWithCovarianceStamped, self.initial_pose_callback)
+ self.goal_subscriber = rospy.Subscriber(
+ 'move_base/status', GoalStatusArray, self.goalCntCallback)
+
+ self.RobotVersion = 1.0
+ self.SystemVersion = 1.0
+ self.is_running = False
+ self.thread = None
+ self.LastError = None
+ self.directory = "~/mercury_x1_ros/src/turn_on_mercury_robot/map"
+
+ self.pose = PoseStamped()
+ self.pose_array = PoseArray()
+ self.pose_stamped = PoseStamped()
+
+ # getPosition
+ self.tf_buffer = tf2_ros.Buffer()
+ self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
+
+ self.point_count = 0
+ self.flag_HomePosition = True
+ self.flag_Parking = True
+ self.flag_Charge = True
+ self.flag_Load = True
+ self.flag_Unload = True
+
+ def getRobotVersion(self):
+ return self.RobotVersion
+
+ def getSystemVersion(self):
+ return self.SystemVersion
+
+ # voltage Callback
+ def voltage_callback(self, msg):
+ return msg.data
+
+ def initial_pose_callback(self, msg):
+ # rospy.loginfo("Received Initial Pose:\n%s", msg)
+ return msg
+
+ # 回调函数订阅话题move_base_simple/goal_temp，填入PoseArray
+ def goalCntCallback(self, goal_msg):
+
+ self.pose_stamped.header = goal_msg.header
+ self.pose_stamped.pose = goal_msg.pose
+
+ # self.pose_array.header.append(pose_stamped.pose)
+ self.pose_array.poses.append(self.pose_stamped.pose)
+
+ def setpoint(self, identifier, xGoal, yGoal, orientation_z, orientation_w):
+ self.pose = PoseStamped()
+ self.pose.header.seq = self.point_count
+ self.pose.header.stamp = rospy.Time.now()
+ self.pose.header.frame_id = 'map'
+ self.pose.pose.position.x = xGoal
+ self.pose.pose.position.y = yGoal
+ self.pose.pose.position.z = 0.0
+ self.pose.pose.orientation.x = 0.0
+ self.pose.pose.orientation.y = 0.0
+ self.pose.pose.orientation.z = orientation_z
+ self.pose.pose.orientation.w = orientation_w
+ # 发布新的点位
+ rospy.sleep(1)
+ self.pub_tempPose.publish(self.pose)
+ self.point_count += 1
+ rospy.loginfo(' {} 对应 {} 号点位 pose: {}'.format(
+ identifier, self.point_count, self.pose))
+ return self.point_count
+
+ # 重置点位
+ def clearPosition(self):
+ self.pose_array.poses = []
+ self.point_count = 0
+ self.flag_HomePosition = True
+ self.flag_Parking = True
+ self.flag_Charge = True
+ self.flag_Load = True
+ self.flag_Unload = True
+ rospy.loginfo('clearPosition')
+
+ def getPose(self, identifier, point_index):
+ if point_index > 0:
+ if point_index <= len(self.pose_array.poses):
+ curGoalIdx_ = point_index % len(self.pose_array.poses)
+ pose = self.pose_array.poses[curGoalIdx_-1]
+ rospy.loginfo('{} Retrieving {} point pose: {}' .format(
+ identifier, point_index, pose))
+
+ def goTopoint(self, identifier, point_index):
+ if point_index > 0:
+ if point_index <= len(self.pose_array.poses):
+ curGoalIdx_ = point_index % len(self.pose_array.poses)
+ goal = PoseStamped()
+ goal.header = self.pose_array.header
+ goal.pose = self.pose_array.poses[curGoalIdx_-1]
+ self.goal_pub.publish(goal)
+ rospy.loginfo('Going to {} point:{}'.format(
+ identifier, goal.pose))
+
+ def setHomePosition(self, xGoal, yGoal, orientation_z, orientation_w):
+ global point_HomePositionCount
+ if self.flag_HomePosition == True:
+ self.flag_HomePosition = False
+ # Returns the navigation point count
+ point_HomePositionCount = self.setpoint(
+ "HomePosition", xGoal, yGoal, orientation_z, orientation_w)
+ else:
+ print(
+ "The navigation point has been set. clearPosition() is required to change the navigation.")
+
+ def setParking(self, xGoal, yGoal, orientation_z, orientation_w):
+ global point_ParkingCount
+ if self.flag_Parking == True:
+ self.flag_Parking = False
+ # Returns the navigation point count
+ point_ParkingCount = self.setpoint(
+ "Parking", xGoal, yGoal, orientation_z, orientation_w)
+ else:
+ print(
+ "The navigation point has been set. clearPosition() is required to change the navigation.")
+
+ def setCharge(self, xGoal, yGoal, orientation_z, orientation_w):
+ global point_ChargeCount
+ if self.flag_Charge == True:
+ self.flag_Charge = False
+ # Returns the navigation point count
+ point_ChargeCount = self.setpoint(
+ "Charge", xGoal, yGoal, orientation_z, orientation_w)
+ else:
+ print(
+ "The navigation point has been set. clearPosition() is required to change the navigation.")
+
+ def setLoad(self, xGoal, yGoal, orientation_z, orientation_w):
+ global point_LoadCount
+ if self.flag_Load == True:
+ self.flag_Load = False
+ # Returns the navigation point count
+ point_LoadCount = self.setpoint(
+ "Load", xGoal, yGoal, orientation_z, orientation_w)
+ else:
+ print(
+ "The navigation point has been set. clearPosition() is required to change the navigation.")
+
+ def setUnload(self, xGoal, yGoal, orientation_z, orientation_w):
+ global point_UnloadCount
+ if self.flag_Unload == True:
+ self.flag_Unload = False
+ # Returns the navigation point count
+ point_UnloadCount = self.setpoint(
+ "Unload", xGoal, yGoal, orientation_z, orientation_w)
+ else:
+ print(
+ "The navigation point has been set. clearPosition() is required to change the navigation.")
+
+ # HomePosition
+ def getHomePosition(self):
+ self.getPose("HomePosition", point_HomePositionCount)
+
+ def goToHomePosition(self):
+ self.goTopoint("HomePosition", point_HomePositionCount)
+
+ # Parking
+ def getParking(self):
+ self.getPose("Parking", point_ParkingCount)
+
+ def goToParking(self):
+ self.goTopoint("Parking", point_ParkingCount)
+
+ # Charge
+ def getCharge(self):
+ self.getPose("Charge", point_ChargeCount)
+
+ def goToCharge(self):
+ self.goTopoint("Charge", point_ChargeCount)
+
+ # Load
+ def getLoad(self):
+ self.getPose("Load", point_LoadCount)
+
+ def goToLoad(self):
+ self.goTopoint("Load", point_LoadCount)
+
+ # Unload
+ def getUnload(self):
+ self.getPose("Unload", point_UnloadCount)
+
+ def goToUnload(self):
+ self.goTopoint("Unload", point_UnloadCount)
+
+ # init robot pose AMCL
+ def set_pose(self, xGoal, yGoal, orientation_z, orientation_w, covariance):
+ pose = PoseWithCovarianceStamped()
+ pose.header.seq = 0
+ pose.header.stamp.secs = 0
+ pose.header.stamp.nsecs = 0
+ pose.header.frame_id = 'map'
+ pose.pose.pose.position.x = xGoal
+ pose.pose.pose.position.y = yGoal
+ pose.pose.pose.position.z = 0.0
+ q = quaternion_from_euler(0, 0, 1.57)
+ pose.pose.pose.orientation.x = 0.0
+ pose.pose.pose.orientation.y = 0.0
+ pose.pose.pose.orientation.z = orientation_z
+ pose.pose.pose.orientation.w = orientation_w
+ pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0, covariance]
+ rospy.sleep(1)
+ self.pub_setpose.publish(pose)
+ rospy.loginfo('Published robot pose: %s' % pose)
+
+ # move_base
+ def moveToGoal(self, xGoal, yGoal, orientation_z, orientation_w):
+ ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+ while (not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
+
+ sys.exit(0)
+
+ goal = MoveBaseGoal()
+ goal.target_pose.header.frame_id = "map"
+ goal.target_pose.header.stamp = rospy.Time.now()
+ goal.target_pose.pose.position = Point(xGoal, yGoal, 0)
+ goal.target_pose.pose.orientation.x = 0.0
+ goal.target_pose.pose.orientation.y = 0.0
+ goal.target_pose.pose.orientation.z = orientation_z
+ goal.target_pose.pose.orientation.w = orientation_w
+
+ rospy.loginfo("Sending goal location ...")
+ ac.send_goal(goal)
+
+ ac.wait_for_result(rospy.Duration(60))
+
+ if (ac.get_state() == GoalStatus.SUCCEEDED):
+ rospy.loginfo("You have reached the destination")
+ return True
+ else:
+ rospy.loginfo("The robot failed to reach the destination")
+ return False
+
+ def shutdown(self):
+ rospy.loginfo("Quit program")
+ rospy.sleep()
+
+ # speed command
+ def pub_vel(self, x, y, theta):
+ twist = Twist()
+ twist.linear.x = x
+ twist.linear.y = y
+ twist.linear.z = 0
+ twist.angular.x = 0
+ twist.angular.y = 0
+ twist.angular.z = theta
+ self.pub.publish(twist)
+
+ def vel_control(self, direction=[0, 0, 0], speed=0.0, control_time=0.0):
+ """
+ Function to control velocity.
+
+ Parameters:
+ - direction (list): An array containing three elements representing the direction of motion.
+ - speed (float): Speed value.
+ - time (float): Time value.
+
+ Raises:
+ - ValueError: If the direction is not a list containing three elements.
+
+ Usage:
+ >>> vel_control([1, 0, 0], 0.2, 5)
+ """
+ duration, vel = control_time, speed
+ start_time = time.time()
+ # Motion control
+ if isinstance(direction, list) and len(direction) == 3:
+ while (time.time() - start_time) < duration:
+ self.pub_vel(direction[0] * abs(vel), direction[1]
+ * abs(vel), direction[2] * abs(vel))
+ self.pub_vel(0, 0, 0)
+ else:
+ print("Direction should be a list containing three elements")
+
+ def turnRight(self, speed, control_time):
+ self.vel_control([0, 0, -1], speed, control_time)
+
+ def turnLeft(self, speed, control_time):
+ self.vel_control([0, 0, 1], speed, control_time)
+
+ def goStraight(self, speed, control_time):
+ self.vel_control([1, 0, 0], speed, control_time)
+
+ def goBack(self, speed, control_time):
+ self.vel_control([-1, 0, 0], speed, control_time)
+
+ def stop(self):
+ self.pub_vel(0, 0, 0)
+
+ def startMapping(self):
+ try:
+ launch_command = "roslaunch turn_on_tringai_robot mapping.launch"
+ subprocess.run(
+ ['gnome-terminal', '-e', f"bash -c '{launch_command}; exec $SHELL'"])
+ except subprocess.CalledProcessError as e:
+ self.LastError = sys.exc_info()
+ print(e)
+
+ def stopMapping(self):
+ try:
+ # Kill the corresponding process
+ close_command = "ps -ef | grep -E " + "mapping.launch" + \
+ " | grep -v 'grep' | awk '{print $2}' | xargs kill -2"
+ subprocess.run(close_command, shell=True)
+ except subprocess.CalledProcessError as e:
+ self.LastError = sys.exc_info()
+ print(e)
+
+ def deleteMap(self):
+ files_to_delete = glob.glob(os.path.join(self.directory, "map.pgm")) + \
+ glob.glob(os.path.join(self.directory, "map.yaml"))
+ try:
+ for file_to_delete in files_to_delete:
+ os.remove(file_to_delete)
+ print(f"Deleted file: {file_to_delete}")
+ except Exception as e:
+ self.LastError = sys.exc_info()
+ print(e)
+
+ def agvOn(self):
+ try:
+ # Start lidar and odometer communication
+ launch_command = "roslaunch turn_on_mercury_robot turn_on_mercury_robot.launch"
+ subprocess.run(
+ ['gnome-terminal', '-e', f"bash -c '{launch_command}; exec $SHELL'"])
+ except Exception as e:
+ self.LastError = sys.exc_info()
+ print(e)
+
+ def agvOff(self):
+ try:
+ # Kill the corresponding process
+ close_command = "ps -ef | grep -E " + "turn_on_mercury_robot.launch" + \
+ " | grep -v 'grep' | awk '{print $2}' | xargs kill -2"
+ subprocess.run(close_command, shell=True)
+ except Exception as e:
+ self.LastError = sys.exc_info()
+ print(e)
+
+ def isAgvOn(self):
+ try:
+ # Check whether there is a corresponding process
+ process_check_command = "ps -ef | grep -E 'turn_on_tringai_robot.launch' | grep -v 'grep'"
+ result = subprocess.run(
+ process_check_command, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
+ if len(result.stdout) > 0:
+ return True
+ else:
+ return False
+ except Exception as e:
+ self.LastError = sys.exc_info()
+ print(e)
+
+ def batteryState(self):
+ voltage_data = self.voltage_callback(None)
+ return voltage_data
+
+ def initPosition(self, x_goal, y_goal, orientation_z, orientation_w, covariance):
+ try:
+ self.set_pose(x_goal, y_goal, orientation_z,
+ orientation_w, covariance)
+ except Exception as e:
+ self.LastError = sys.exc_info()
+
+ def getPosition(self):
+ # Position = self.initial_pose_callback(None)
+ # return Position
+ try:
+ transform = self.tf_buffer.lookup_transform(
+ "map", "base_up", rospy.Time(0), rospy.Duration(1.0))
+ translation = transform.transform.translation
+ rotation = transform.transform.rotation
+
+ x = translation.x
+ y = translation.y
+
+ euler = tf_conversions.transformations.euler_from_quaternion(
+ [rotation.x, rotation.y, rotation.z, rotation.w])
+
+ tw = euler[2] # Yaw
+
+ rospy.loginfo("X position{}".format(x))
+ rospy.loginfo("Y position{}".format(y))
+ rospy.loginfo("TW orientation{}".format(tw))
+
+ return x, y, tw
+
+ except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
+ rospy.logerr("TF transformation query failed:{}".format(e))
+
+ def goToPosition(self, goal_x, goal_y, orientation_z, orientation_w):
+ flag_feed_goalReached = self.moveToGoal(
+ goal_x, goal_y, orientation_z, orientation_w)
+ return flag_feed_goalReached
+
+ def pause(self):
+ if not self.is_running:
+ self.is_running = True
+ self.thread = threading.Thread(
+ target=self.stop_navigation, daemon=True)
+ self.thread.start()
+
+ def stop_navigation(self):
+ while self.is_running:
+ goal_id = GoalID()
+ self.pub_cancel.publish(goal_id)
+ twist = Twist()
+ twist.linear.x = 0.0
+ twist.linear.y = 0.0
+ twist.linear.z = 0.0
+ twist.angular.x = 0.0
+ twist.angular.y = 0.0
+ twist.angular.z = 0.0
+ self.pub.publish(twist)
+ rospy.sleep(0.1)
+
+ def unpause(self):
+ self.is_running = False
+ if self.thread is not None:
+ self.thread.join()
+
+ def getLastError(self):
+ if self.LastError is not None:
+ exc_type, exc_value, exc_traceback = self.LastError
+ print(f"Exception Type: {exc_type}")
+ print(f"Exception Value: {exc_value}")
+ print("Exception Traceback:")
+ traceback_str = "\n".join(self.LastError.format_tb(exc_traceback))
+ print(traceback_str)
+ else:
+ print("No recent errors.")
+ return self.LastError
+
+ def startNavigation(self):
+ try:
+ # Start lidar and odometer communication
+ launch_command = "roslaunch myagv_navigation navigation_active.launch"
+ subprocess.run(
+ ['gnome-terminal', '-e', f"bash -c '{launch_command}; exec $SHELL'"])
+ except Exception as e:
+ self.LastError = sys.exc_info()
+ print(e)
+
+
+if __name__ == '__main__':
+ # init navigation
+ map_navigation = MapNavigation()
+
+ print(map_navigation.getRobotVersion())
+ print(map_navigation.getSystemVersion())
+
+ print(map_navigation.isAgvOn())
+
+ # map_navigation.getLastError()
+
+ # map_navigation.initPosition(1.1, -0.22, 0.7, 0.99,0.55)
+
+ # print(map_navigation.getPosition())
+
+ # map_navigation.getLastError()
+
+ # map_navigation.turnRight(0.2,5)
+
+ # map_navigation.goStraight(0.2,5)
diff --git a/pymycobot/mercurysocket.py b/pymycobot/mercurysocket.py
index b6a370d..9473841 100644
--- a/pymycobot/mercurysocket.py
+++ b/pymycobot/mercurysocket.py
@@ -5,7 +5,6 @@
import threading
from pymycobot.mercury_api import MercuryCommandGenerator
-from pymycobot.error import calibration_parameters
class MercurySocket(MercuryCommandGenerator):
@@ -16,21 +15,24 @@ def __init__(self, ip, netport=9000, debug=False):
netport: Server port(default 9000)
"""
super(MercurySocket, self).__init__(debug)
- self.calibration_parameters = calibration_parameters
self.SERVER_IP = ip
self.SERVER_PORT = netport
self.sock = self.connect_socket()
self.lock = threading.Lock()
- self.read_threading = threading.Thread(target=self.read_thread, daemon=True)
+ self.lock_out = threading.Lock()
+ self.read_threading = threading.Thread(
+ target=self.read_thread, args=("socket", ))
+ self.read_threading.daemon = True
self.read_threading.start()
+ self.get_limit_switch()
def connect_socket(self):
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.connect((self.SERVER_IP, self.SERVER_PORT))
return sock
-
+
def open(self):
self.sock = self.connect_socket()
-
+
def close(self):
self.sock.close()
diff --git a/pymycobot/myagv.py b/pymycobot/myagv.py
index e3bf3f0..c802e15 100644
--- a/pymycobot/myagv.py
+++ b/pymycobot/myagv.py
@@ -1,209 +1,296 @@
import enum
+import threading
import serial
import time
import struct
import logging
-from pymycobot.log import setup_logging
-from pymycobot.common import DataProcessor
-from pymycobot.error import calibration_parameters
+import logging.handlers
+
+
+def setup_logging(name: str = __name__, debug: bool = False) -> logging.Logger:
+ debug_formatter = logging.Formatter(
+ fmt="%(asctime)s %(levelname)s [%(name)s] - %(message)s",
+ datefmt="%H:%M:%S",
+ )
+ logger = logging.getLogger(name)
+ stream_handler = logging.StreamHandler()
+ stream_handler.setFormatter(debug_formatter)
+ if debug is True:
+ logger.addHandler(stream_handler)
+ logger.setLevel(logging.INFO) # 100M日志
+ file_handler = logging.handlers.RotatingFileHandler(
+ filename="python_debug.log", maxBytes=100 * 1024 * 1024, backupCount=1
+ )
+ file_handler.setFormatter(debug_formatter)
+ logger.addHandler(file_handler)
+
+ else:
+ logger.setLevel(logging.DEBUG)
+
+ return logger
class ProtocolCode(enum.Enum):
- HEADER = 0xFE
- RESTORE = [0x01, 0x00]
- SET_LED = [0x01, 0x02]
- GET_FIRMWARE_VERSION = [0x01, 0x03]
- GET_MOTORS_CURRENT = [0x01, 0x04]
- GET_BATTERY_INFO = [0x01, 0x05]
- SET_GYRO_STATE = [0x01, 0x07]
- GET_GYRO_STATE = [0x01, 0x08]
- GET_MODIFIED_VERSION = [0x01, 0x09]
-
-
-class MyAgv(DataProcessor):
- def __init__(self, port="/dev/ttyAMA0", baudrate="115200", timeout=0.1, debug=False):
- self.debug = debug
- setup_logging(self.debug)
- self.log = logging.getLogger(__name__)
- self._serial_port = serial.Serial()
- self._serial_port.port = port
- self._serial_port.baudrate = baudrate
- self._serial_port.timeout = timeout
+ HEADER = (0xFE, 0xFE)
+ RESTORE = (0x01, 0x00)
+ SET_LED = (0x01, 0x02)
+ SET_LED_MODE = (0x01, 0x0A)
+ GET_FIRMWARE_VERSION = (0x01, 0x03)
+ GET_MODIFIED_VERSION = (0x01, 0x09)
+ SET_GYRO_STATE = (0x01, 0x07)
+ GET_GYRO_STATE = (0x01, 0x08)
+ GET_MCU_INFO = (0x01, 0x0B)
+ UNDEFINED = ()
+
+
+class SerialStreamProtocol(object):
+
+ def __init__(self, port="/dev/ttyAMA0", baudrate=115200, timeout=0.1):
+ self._serial_port = serial.Serial(port=port, baudrate=baudrate, timeout=timeout)
self._serial_port.rts = False
- self._serial_port.open()
- self.__movement = False
- def _write(self, command):
- self._serial_port.reset_input_buffer()
- self.log.debug("_write: {}".format([hex(i) for i in command]))
- self._serial_port.write(command)
+ def open(self):
+ if self._serial_port.is_open is False:
+ self._serial_port.open()
+
+ def close(self):
+ if self._serial_port.is_open is True:
+ self._serial_port.close()
+
+ def flush(self):
self._serial_port.flush()
- def _read(self, command) -> bytes:
- datas = b''
- k = 0
- pre = 0
- end = 5
- t = time.time()
- if command[k - 1] == 29:
- end = 28
- elif command[k - 1] == 44:
- end = 43
- while time.time() - t < 0.2:
- data = self._serial_port.read()
- k += 1
- if len(datas) == 4:
- if datas[-2] == 0x01 and datas[-1] == 0x05:
- end = 7
- datas += data
-
- elif len(datas) == end:
- datas += data
- break
- elif len(datas) > 4:
- datas += data
+ def write(self, data):
+ self._serial_port.write(data)
+ self._serial_port.flush()
- elif len(datas) >= 2:
- data_len = struct.unpack("b", data)[0]
- if command[-1] == 29 or command[-1] == 44 or data_len == command[k - 1]:
- datas += data
- else:
- datas = b''
- k = 0
- pre = 0
- elif data == b"\xfe":
- if datas == b'':
- datas += data
- if k != 1:
- k = 1
- pre = k
- else:
- if k - 1 == pre:
- datas += data
- else:
- datas = b"\xfe"
- k = 1
- pre = 0
- else:
- datas = b''
- self.log.debug("_read: {}".format([hex(data) for data in datas]))
- return datas
+ def read(self, size):
+ return self._serial_port.read(size)
- def _mesg(self, genre, *args, **kwargs):
- """
+ def readall(self):
+ return self._serial_port.read_all()
- Args:
- genre: command type (Command)
- *args: other data.
- It is converted to octal by default.
- If the data needs to be encapsulated into hexadecimal,
- the array is used to include them. (Data cannot be nested)
- **kwargs: support `has_reply`
- has_reply: Whether there is a return value to accept.
- """
- has_reply = kwargs.get("has_reply", None)
- real_command = self._process_data_command(genre, self.__class__.__name__, args)
- command = [
- ProtocolCode.HEADER.value,
- ProtocolCode.HEADER.value,
- ]
- if isinstance(genre, list):
- for data in genre:
- command.append(data)
+
+class CommandProtocol(object):
+
+ def _process_data_command(self, args):
+ if not args:
+ return []
+
+ processed_args = []
+ for index in range(len(args)):
+ if isinstance(args[index], list):
+ data = self._encode_int16(args[index])
+ processed_args.extend(data)
+ else:
+ processed_args.append(args[index])
+
+ return processed_args
+
+ def _flatten(self, datas):
+ flat_list = []
+ for item in datas:
+ if not isinstance(item, list):
+ flat_list.append(item)
+ else:
+ flat_list.extend(self._flatten(item))
+ return flat_list
+
+ @classmethod
+ def _float(cls, number, decimal=2):
+ return round(number / 10 ** decimal, 2)
+
+ @classmethod
+ def _encode_int16(cls, data):
+ if isinstance(data, int):
+ return [
+ ord(i) if isinstance(i, str) else i
+ for i in list(struct.pack(">h", data))
+ ]
else:
- command.append(genre)
- command.append(real_command)
- command = self._flatten(command)
- if genre == ProtocolCode.SET_LED.value:
- command.append(sum(command[2:]) & 0xff)
- elif genre == ProtocolCode.GET_FIRMWARE_VERSION.value:
- command.append(4)
- elif genre == ProtocolCode.GET_MOTORS_CURRENT.value:
- command.append(5)
- elif genre == ProtocolCode.GET_BATTERY_INFO.value:
- command.append(6)
+ res = []
+ for v in data:
+ t = cls._encode_int16(v)
+ res.extend(t)
+ return res
+
+ @classmethod
+ def _decode_int16(cls, data):
+ return struct.unpack(">h", data)[0]
+
+
+class MyAgv(SerialStreamProtocol, CommandProtocol):
+
+ def __init__(self, comport, baudrate=115200, timeout=0.1, debug=False):
+ super().__init__(comport, baudrate, timeout)
+ self.__movement = False
+ self.__command_buffer_table = {}
+ self.__mutex = threading.Lock()
+ self.log = setup_logging(name=self.__class__.__name__, debug=debug)
+ self._command_read_thread = threading.Thread(target=self._read_command_buffer_thread, daemon=True)
+ self._command_read_thread.start()
+
+ @classmethod
+ def __is_complete_command(cls, command):
+ return sum(command[2:-1]) & 0xff == command[-1] and len(command) > 5
+
+ def _read_command_buffer(self):
+ previous_frame = b""
+ is_record = False
+ commands = b"\xfe\xfe"
+
+ while True:
+ current_frame = self.read(1)
+
+ if current_frame == b"\xfe" and previous_frame == b"\xfe" and is_record is False:
+ is_record = True
+ continue
+
+ previous_frame = current_frame
+ if is_record is False:
+ continue
+
+ commands += current_frame
+ if sum(commands[2:-1]) & 0xff == commands[-1] and len(commands) > 5:
+ break
+ return commands
+
+ def _read_command_buffer_thread(self):
+ while True:
+ command_buffers = self._read_command_buffer()
+ if self.__is_complete_command(command_buffers):
+ self.log.info(f"write: {' '.join(f'{x:02x}' for x in command_buffers)}")
+ genre = tuple(command_buffers[2:4])
+ if genre == (128, 128):
+ genre = ProtocolCode.GET_MCU_INFO.value
+ self.__command_buffer_table[genre] = (time.perf_counter(), command_buffers)
+ time.sleep(0.008)
+
+ def _compose_complete_command(self, genre: ProtocolCode, params): # packing command
+ command_args = self._process_data_command(params)
+ command_args = self._flatten(command_args)
+
+ command = [*ProtocolCode.HEADER.value]
+ if isinstance(genre.value, tuple):
+ command.extend(genre.value)
else:
- # del command[2]
- command.append(sum(command[2:]) & 0xff)
- self._write(command)
- if has_reply:
- data = self._read(command)
- if data:
- if genre in [ProtocolCode.GET_FIRMWARE_VERSION.value]:
- return self._int2coord(data[4])
- elif genre == ProtocolCode.GET_MOTORS_CURRENT.value:
- return self._decode_int16(data[4:6])
- elif genre == ProtocolCode.GET_BATTERY_INFO.value:
- byte_1 = bin(data[4])[2:]
- res = []
- while len(byte_1) != 6:
- byte_1 = "0" + byte_1
- res.append(byte_1)
- res.append(self._int2coord(data[5]))
- res.append(self._int2coord(data[6]))
- if byte_1[0] == "0":
- res[-1] = 0
- elif byte_1[1] == "0":
- res[1] = 0
- return res
- return data[4]
-
- return None
-
- def set_led(self, mode, R, G, B):
+ command.append(genre.value)
+
+ command.extend(command_args)
+ command.append(sum(command[2:]) & 0xff)
+ return command
+
+ def _parse_reply_instruction(self, genre: ProtocolCode): # unpacking command
+ timestamp, reply_data = self.__command_buffer_table.get(genre.value, (time.perf_counter(), []))
+ if not reply_data:
+ return None
+
+ self.log.info(f"read : {' '.join(f'{x:02x}' for x in reply_data)}")
+ if genre == ProtocolCode.GET_FIRMWARE_VERSION:
+ return self._float(reply_data[4], 1)
+ elif genre == ProtocolCode.GET_MCU_INFO:
+ if len(reply_data) < 30:
+ return None
+ index = 0
+ res = []
+ datas = reply_data[2:][:-1] # header and footer frames are not counted
+ while index < len(datas):
+ if index in range(0, 3):
+ res.append(datas[index])
+ index += 1
+
+ elif index in range(3, 15, 2):
+ data = self._decode_int16(datas[index:index + 2])
+ res.append(self._float(data, 2))
+ index += 2
+
+ elif index == 15:
+ binary = bin(datas[index])[2:]
+ binary = binary.zfill(6)
+ res.append(binary)
+ index += 1
+
+ elif index in (16, 17):
+ res.append(datas[index] / 10)
+ index += 1
+
+ elif index in range(18, 26, 2):
+ res.append(self._float(self._decode_int16(datas[index:index + 2]), 3))
+ index += 2
+
+ elif index in range(26, 32, 2):
+ res.append(self._float(self._decode_int16(datas[index:index + 2]), 3))
+ index += 2
+
+ elif index in range(32, 42, 1):
+ res.append(datas[index])
+ index += 1
+ else:
+ index += 1
+
+ return res
+ return reply_data[4]
+
+ def _merge(self, genre: ProtocolCode, *args, has_reply=False, in_buffer=False):
+ if in_buffer is False:
+ real_command = self._compose_complete_command(genre, args)
+ self.log.info(f"write: {' '.join(f'{x:02x}' for x in real_command)}")
+ with self.__mutex:
+ self.write(real_command)
+ if has_reply is False:
+ return None
+ time.sleep(0.1)
+ return self._parse_reply_instruction(genre)
+
+ def set_led(self, mode, r, g, b):
"""Set up LED lights
Args:
- mode (int): 1 - Set LED light color. 2 - Set the LED light to blink
- R (int): 0 ~ 255
- G (int): 0 ~ 255
- B (int): 0 ~ 255
+ mode (int):
+ 1 - Set LED light color.
+ 2 - Set the LED light to blink
+ r (int): 0 ~ 255
+ g (int): 0 ~ 255
+ b (int): 0 ~ 255
"""
- calibration_parameters(class_name=self.__class__.__name__, rgb=[R, G, B], led_mode=mode)
- return self._mesg(ProtocolCode.SET_LED.value, mode, R, G, B)
+ if mode not in (1, 2):
+ raise ValueError("mode must be 1 or 2")
- def get_firmware_version(self):
- """Get firmware version number
- """
- return self._mesg(ProtocolCode.GET_FIRMWARE_VERSION.value, has_reply=True)
+ if r not in range(256):
+ raise ValueError("r must be 0 ~ 255")
- def get_motors_current(self):
- """Get the total current of the motor
- """
- return self._mesg(ProtocolCode.GET_MOTORS_CURRENT.value, has_reply=True)
+ if g not in range(256):
+ raise ValueError("g must be 0 ~ 255")
- def get_battery_info(self):
- """Read battery information
-
- Return:
- list : [battery_data, battery_1_voltage, battery_2_voltage].
- battery_data:
- A string of length 6, represented from left to right:
- bit5, bit4, bit3, bit2, bit1, bit0.
-
- bit5 : Battery 2 is inserted into the interface 1 means inserted, 0 is not inserted.
- bit4 : Battery 1 is inserted into the interface, 1 means inserted, 0 is not inserted.
- bit3 : The adapter is plugged into the interface 1 means plugged in, 0 not plugged in.
- bit2 : The charging pile is inserted into the interface, 1 means plugged in, 0 is not plugged in.
- bit1 : Battery 2 charging light 0 means off, 1 means on.
- bit0 : Battery 1 charging light, 0 means off, 1 means on.
- battery_1_voltage : Battery 1 voltage in volts.
- battery_2_voltage : Battery 2 voltage in volts.
+ if b not in range(256):
+ raise ValueError("b must be 0 ~ 255")
+
+ return self._merge(ProtocolCode.SET_LED, mode, r, g, b)
+
+ def set_led_mode(self, mode: int):
+ """Set the LED light mode
+
+ Args:
+ mode (int): 0 - charging mode, 1 - DIY mode
"""
- return self._mesg(ProtocolCode.GET_BATTERY_INFO.value, has_reply=True)
+ if mode not in [0, 1]:
+ raise ValueError("mode must be 0 or 1")
+ return self._merge(ProtocolCode.SET_LED_MODE, mode)
def __basic_move_control(self, *genre, timeout: int = 5):
t = time.time()
self.__movement = True
- while time.time() - t < timeout and self.__movement is True:
- self._mesg(*genre)
- time.sleep(0.05)
+ while time.time() - t < timeout:
+ if self.__movement is False:
+ break
+ self._merge(ProtocolCode.UNDEFINED, *genre)
+ time.sleep(0.1)
self.stop()
def go_ahead(self, speed: int, timeout: int = 5):
"""
- Control the car to move forward.
- Send control commands every 100ms.
+ Control the car to move forward.
+ Send control commands every 100ms.
with a default motion time of 5 seconds.
Args:
@@ -216,7 +303,7 @@ def go_ahead(self, speed: int, timeout: int = 5):
def retreat(self, speed, timeout=5):
"""
- Control the car back. Send control commands every 100ms.
+ Control the car back. Send control commands every 100ms.
with a default motion time of 5 seconds
Args:
@@ -229,7 +316,7 @@ def retreat(self, speed, timeout=5):
def pan_left(self, speed, timeout=5):
"""
- Control the car to pan to the left.
+ Control the car to pan to the left.
Send control commands every 100ms. with a default motion time of 5 seconds
Args:
@@ -238,13 +325,13 @@ def pan_left(self, speed, timeout=5):
"""
if not (0 < speed < 128):
raise ValueError("pan_left_speed must be between 1 and 127")
- self.__basic_move_control(128, 128, 128 + speed, timeout=timeout)
+ self.__basic_move_control(128, 128 + speed, 128, timeout=timeout)
def pan_right(self, speed: int, timeout=5):
"""
- Control the car to pan to the right.
+ Control the car to pan to the right.
Send control commands every 100ms. with a default motion time of 5 seconds
-
+
Args:
speed (int): 1 ~ 127
timeout (int): default 5 s.
@@ -255,9 +342,9 @@ def pan_right(self, speed: int, timeout=5):
def clockwise_rotation(self, speed: int, timeout=5):
"""
- Control the car to rotate clockwise.
+ Control the car to rotate clockwise.
Send control commands every 100ms. with a default motion time of 5 seconds
-
+
Args:
speed (int): 1 ~ 127
timeout (int): default 5 s.
@@ -268,7 +355,7 @@ def clockwise_rotation(self, speed: int, timeout=5):
def counterclockwise_rotation(self, speed: int, timeout=5):
"""
- Control the car to rotate counterclockwise.
+ Control the car to rotate counterclockwise.
Send control commands every 100ms. with a default motion time of 5 seconds
Args:
speed (int): 1 ~ 127
@@ -280,15 +367,12 @@ def counterclockwise_rotation(self, speed: int, timeout=5):
def stop(self):
"""stop-motion"""
- if self.__movement is True:
- self._mesg(128, 128, 128)
- self.__movement = False
+ self.__movement = False
+ self._merge(ProtocolCode.UNDEFINED, 128, 128, 128)
- def get_mcu_info(self, version: float = None) -> list:
+ def get_mcu_info(self):
"""
Get MCU information
- Args:
- version (float): firmware version, default None, auto-detect
Returns:
MCU Version(list):
version 1.0:
@@ -310,60 +394,11 @@ def get_mcu_info(self, version: float = None) -> list:
20-24: stall state 1 - stalled, 0 - normal
24-28: encoder status 1 - normal, 0 - abnormal
"""
- if version is None:
- version = self.get_firmware_version()
-
- if version == 1.0:
- data_len = 29
- else:
- data_len = 44
-
- res = []
- index = 0
- datas = self._read([0xfe, 0xfe, data_len])
- if not datas:
- return res
-
- datas = datas[2:][:-1] # header and footer frames are not counted
- while index < len(datas):
- if index in range(0, 3):
- res.append(datas[index])
- index += 1
-
- elif index in range(3, 15, 2):
- res.append(self._decode_int16(datas[index:index + 2]))
- index += 2
-
- elif index == 15:
- byte_1 = bin(datas[index])[2:]
- while len(byte_1) < 6:
- byte_1 = "0" + byte_1
- res.append(byte_1)
- index += 1
-
- elif index in (16, 17):
- res.append(datas[index] / 10)
- index += 1
-
- elif index in range(18, 26, 2):
- res.append(self._int2angle(self._decode_int16(datas[index:index + 2])))
- index += 2
-
- elif index in range(26, 32, 2):
- res.append(self._int2angle(self._decode_int16(datas[index:index + 2])))
- index += 2
-
- elif index in range(32, 42, 1):
- res.append(datas[index])
- index += 1
- else:
- index += 1
-
- return res
+ return self._merge(ProtocolCode.GET_MCU_INFO, has_reply=True, in_buffer=True)
def restore(self):
"""Motor stall recovery"""
- self._mesg(ProtocolCode.RESTORE.value, 1)
+ self._merge(ProtocolCode.RESTORE, 0x01)
def set_gyro_state(self, state=1):
"""Set gyroscope calibration status (save after power failure)
@@ -373,7 +408,7 @@ def set_gyro_state(self, state=1):
"""
if state not in (0, 1):
raise ValueError("state must be 0 or 1")
- self._mesg(ProtocolCode.SET_GYRO_STATE.value, state)
+ self._merge(ProtocolCode.SET_GYRO_STATE, state)
def get_gyro_state(self):
"""Get gyroscope calibration status
@@ -382,16 +417,12 @@ def get_gyro_state(self):
1 - open
0 - close
"""
- return self._mesg(ProtocolCode.GET_GYRO_STATE.value, has_reply=True)
+ return self._merge(ProtocolCode.GET_GYRO_STATE, has_reply=True)
- def get_modified_version(self):
- return self._mesg(ProtocolCode.GET_MODIFIED_VERSION.value, has_reply=True)
-
- # def get_battery_voltage(self, num=1):
- # """Get battery voltage
+ def get_firmware_version(self):
+ """Get firmware version number"""
+ return self._merge(ProtocolCode.GET_FIRMWARE_VERSION, has_reply=True)
- # Args:
- # num (int, optional): Battery ID number. Defaults to 1.
- # """
- # mcu_data = self.get_mcu_info()
- # return self._mesg(ProtocolCode.GET_BATTERY_INFO.value, has_reply = True)
+ def get_modified_version(self):
+ """Get modified version number"""
+ return self._merge(ProtocolCode.GET_MODIFIED_VERSION, has_reply=True)
diff --git a/pymycobot/myarm.py b/pymycobot/myarm.py
index fa4d888..cf3603f 100644
--- a/pymycobot/myarm.py
+++ b/pymycobot/myarm.py
@@ -5,14 +5,15 @@
import math
import threading
import logging
+import threading
from pymycobot.log import setup_logging
from pymycobot.generate import CommandGenerator
from pymycobot.common import ProtocolCode, write, read
from pymycobot.error import calibration_parameters
+from pymycobot.sms import sms_sts
-
-class MyArm(CommandGenerator):
+class MyArm(CommandGenerator, sms_sts):
"""MyCobot Python API Serial communication class.
Supported methods:
@@ -64,6 +65,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
self._serial_port.rts = False
self._serial_port.open()
self.lock = threading.Lock()
+ super(sms_sts, self).__init__(self._serial_port, 0)
_write = write
_read = read
@@ -80,7 +82,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyArm, self)._mesg(genre, *args, **kwargs)
command = self._flatten(real_command)
with self.lock:
@@ -153,6 +155,18 @@ def _mesg(self, genre, *args, **kwargs):
else:
r.append(self._int2angle(res[index]))
return r
+ elif genre == ProtocolCode.GET_ANGLES_COORDS_END:
+ r = []
+ for index in range(len(res)):
+ if index < 7:
+ r.append(self._int2angle(res[index]))
+ elif index < 10:
+ r.append(self._int2coord(res[index]))
+ elif index < 13:
+ r.append(self._int2angle(res[index]))
+ else:
+ r.append(res[index])
+ return r
elif genre == ProtocolCode.GET_SOLUTION_ANGLES:
return self._int2angle(res[0])
else:
@@ -215,9 +229,9 @@ def sync_send_angles(self, degrees, speed, timeout=15):
while time.time() - t < timeout:
f = self.is_in_position(degrees, 0)
if f == 1:
- break
+ return 1
time.sleep(0.1)
- return self
+ return 0
def sync_send_coords(self, coords, speed, mode=0, timeout=15):
"""Send the coord in synchronous state and return when the target point is reached
@@ -232,9 +246,9 @@ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
self.send_coords(coords, speed, mode)
while time.time() - t < timeout:
if self.is_in_position(coords, 1) == 1:
- break
+ return 1
time.sleep(0.1)
- return self
+ return 0
# Basic for raspberry pi.
def gpio_init(self):
@@ -333,7 +347,15 @@ def is_in_position(self, data, id=0):
else:
raise Exception("id is not right, please input 0 or 1")
return self._mesg(ProtocolCode.IS_IN_POSITION, id, data_list, has_reply=True)
-
+
+ def get_quick_move_info(self):
+ """_summary_
+ """
+ return self._mesg(ProtocolCode.GET_ANGLES_COORDS_END, has_reply=True)
+
+ def go_home(self):
+ self.send_angles([0,0,0,0,0,0,0], 10)
+
# JOG mode and operation
def jog_angle(self, joint_id, direction, speed):
"""Jog control angle.
diff --git a/pymycobot/myarm_api.py b/pymycobot/myarm_api.py
index 3818bc0..38da7e6 100644
--- a/pymycobot/myarm_api.py
+++ b/pymycobot/myarm_api.py
@@ -1,24 +1,24 @@
# coding=utf-8
from __future__ import division
-
-import sys
-import logging
+import functools
import threading
import time
-
-from pymycobot.common import ProtocolCode, write, read, DataProcessor
+from pymycobot.common import ProtocolCode, DataProcessor
from pymycobot.error import calibration_parameters
-from pymycobot.log import setup_logging
import serial
-class MyArmAPI(DataProcessor):
- """
+def setup_serial_connect(port, baudrate, timeout=None):
+ serial_api = serial.Serial(port=port, baudrate=baudrate, timeout=timeout)
+ serial_api.rts = False
+ if not serial_api.is_open:
+ serial_api.open()
+ return serial_api
- """
- def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
+class MyArmMCProcessor(DataProcessor):
+ def __init__(self, port, baudrate=115200, timeout=0.1, debug=False):
"""
Args:
port : port string
@@ -26,22 +26,65 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
timeout : default 0.1
debug : whether show debug info
"""
- super(MyArmAPI, self).__init__()
- self.calibration_parameters = calibration_parameters
- self._serial_port = serial.Serial()
- self._serial_port.port = port
- self._serial_port.baudrate = baudrate
- self._serial_port.timeout = timeout
- self._serial_port.rts = False
- self._serial_port.open()
+ super(MyArmMCProcessor, self).__init__(debug=debug)
+ self.calibration_parameters = functools.partial(calibration_parameters, class_name=self.__class__.__name__)
+ self._serial_port = setup_serial_connect(port, baudrate, timeout)
self.lock = threading.Lock()
- self._version = sys.version_info[:2][0]
- self.debug = debug
- setup_logging(self.debug)
- self.log = logging.getLogger(__name__)
- _write = write
- _read = read
+ @classmethod
+ def __check_command_integrity(cls, command):
+ """
+ Check the integrity of the command.
+ Instruction format: header + header + length + genre + *data + footer
+
+ :param command: the command to check
+ :return: True if the command is valid, False otherwise
+ """
+ return len(command) >= 5 and command[-1] == ProtocolCode.FOOTER and command[2] == len(command) - 3
+
+ def _read_command_buffer(self, timeout=0.1):
+ commands = b""
+ is_record = False
+ previous_frame = b""
+ stime = time.perf_counter()
+ while time.perf_counter() - stime < timeout:
+ current_frame = self._serial_port.read(1)
+
+ if current_frame == b"\xfe" and previous_frame == b"\xfe" and is_record is False:
+ is_record = True
+ commands = b"\xfe\xfe"
+ continue
+
+ previous_frame = current_frame
+ if is_record is False:
+ continue
+
+ commands += current_frame
+ if self.__check_command_integrity(commands):
+ break
+ return commands
+
+ def _read(self, genre):
+ if genre == ProtocolCode.GET_ROBOT_STATUS:
+ timeout = 90
+ elif genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
+ timeout = 0.05
+ elif genre in [ProtocolCode.INIT_ELECTRIC_GRIPPER, ProtocolCode.SET_GRIPPER_MODE]:
+ timeout = 0.3
+ elif genre in [ProtocolCode.SET_GRIPPER_CALIBRATION]:
+ timeout = 0.4
+ else:
+ timeout = 0.1
+
+ command_buffer = self._read_command_buffer(timeout=timeout)
+ self.log.debug(f"_read : {' '.join(map(lambda x: f'{x:02x}', command_buffer))}")
+ return command_buffer
+
+ def _write(self, command):
+ self.log.debug(f"_write: {' '.join(map(lambda x: f'{x:02x}', command))}")
+ if not self._serial_port.is_open:
+ self._serial_port.open()
+ self._serial_port.write(command)
def _mesg(self, genre, *args, **kwargs):
"""
@@ -54,20 +97,17 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- time.sleep(0.01)
- real_command, has_reply = super(MyArmAPI, self)._mesg(genre, *args, **kwargs)
- command = self._flatten(real_command)
+ real_command, has_reply, _async = super(MyArmMCProcessor, self)._mesg(genre, *args, **kwargs)
with self.lock:
- self._write(command)
+ time.sleep(0.1)
+ self._write(self._flatten(real_command))
if not has_reply:
return None
- timeout = kwargs.get('timeout', None)
- data = self._read(genre, command=command, timeout=timeout, _class=self.__class__.__name__)
+ data = self._read(genre)
res = self._process_received(data, genre, arm=8)
-
- if len(res) == 0:
+ if not res:
return None
return_single_genres = [
@@ -97,15 +137,20 @@ def _mesg(self, genre, *args, **kwargs):
if genre in (ProtocolCode.COBOTX_GET_ANGLE,):
result = self._int2angle(result)
return result
- if genre in (ProtocolCode.GET_ATOM_PRESS_STATUS, ):
+ if genre in (ProtocolCode.GET_ATOM_PRESS_STATUS,):
if self.__class__.__name__ == 'MyArmM':
return res[0]
return res
- elif genre in [
- ProtocolCode.GET_ANGLES, ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE,
- ProtocolCode.GET_JOINTS_COORD
- ]:
+ elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE]:
return [self._int2angle(angle) for angle in res]
+ elif genre == ProtocolCode.GET_JOINTS_COORD:
+ r = []
+ for idx, val in enumerate(res):
+ if idx < 3:
+ r.append(self._int2coord(val))
+ else:
+ r.append(self._int2angle(val))
+ return r
elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
return [self._int2coord(angle) for angle in res]
elif genre == ProtocolCode.GET_ROBOT_ERROR_STATUS:
@@ -124,6 +169,9 @@ def _mesg(self, genre, *args, **kwargs):
else:
return res
+
+class MyArmAPI(MyArmMCProcessor):
+
def get_robot_modified_version(self):
"""Get the bot correction version number
@@ -154,6 +202,8 @@ def set_robot_err_check_state(self, status):
Args:
status (int): 1 open; o close
"""
+ if status not in [0, 1]:
+ raise ValueError("status must be 0 or 1")
self._mesg(ProtocolCode.SET_ROBOT_ERROR_CHECK_STATE, status)
def get_robot_err_check_state(self):
@@ -182,16 +232,21 @@ def get_recv_queue_len(self):
"""The current length of the read receive queue"""
return self._mesg(ProtocolCode.GET_RECV_QUEUE_LENGTH, has_reply=True)
+ def clear_recv_queue(self):
+ """Clear the read receive queue"""
+ return self._mesg(ProtocolCode.CLEAR_RECV_QUEUE)
+
def get_joint_angle(self, joint_id):
"""
- Obtain the current angle of the specified joint, when the obtained angle is 200��,
+ Obtain the current angle of the specified joint, when the obtained angle is 200,
it means that the joint has no communication
Args:
joint_id (int): 0 - 254
Returns:
-
+ angle (int)
"""
+ self.calibration_parameters(joint_id=joint_id)
return self._mesg(ProtocolCode.COBOTX_GET_ANGLE, joint_id, has_reply=True)
def get_joints_angle(self):
@@ -209,7 +264,7 @@ def set_servo_calibrate(self, servo_id):
Args:
servo_id (int): 0 - 254
"""
-
+ self.calibration_parameters(servo_id=servo_id)
self._mesg(ProtocolCode.SET_SERVO_CALIBRATION, servo_id)
def get_servo_encoder(self, servo_id):
@@ -221,6 +276,7 @@ def get_servo_encoder(self, servo_id):
Returns:
encoder (int): 0-4095
"""
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.GET_ENCODER, servo_id, has_reply=True)
def get_servos_encoder_drag(self):
@@ -277,7 +333,7 @@ def set_servo_p(self, servo_id, data):
"""
assert 0 <= data <= 254, "data must be between 0 and 254"
- self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
+ self.calibration_parameters(servo_id=servo_id)
self._mesg(ProtocolCode.SET_SERVO_P, servo_id, data)
def get_servo_p(self, servo_id):
@@ -286,6 +342,7 @@ def get_servo_p(self, servo_id):
Args:
servo_id (int): 0-254
"""
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.GET_SERVO_P, servo_id, has_reply=True)
def set_servo_d(self, servo_id, data):
@@ -296,7 +353,7 @@ def set_servo_d(self, servo_id, data):
data (int): 0-254
"""
assert 0 <= data <= 254, "data must be between 0 and 254"
- self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
+ self.calibration_parameters(servo_id=servo_id)
self._mesg(ProtocolCode.MERCURY_DRAG_TECH_EXECUTE, servo_id, data)
def get_servo_d(self, servo_id):
@@ -305,7 +362,8 @@ def get_servo_d(self, servo_id):
Args:
servo_id (int): 0-254
"""
- return self._mesg(ProtocolCode.GET_SERVO_D, servo_id, has_reply=True)
+ self.calibration_parameters(servo_id=servo_id)
+ return self._mesg(ProtocolCode.GET_SERVO_D, servo_id, has_reply=True, timeout=0.1)
def set_servo_i(self, servo_id, data):
"""Set the proportional factor of the position ring I of the specified servo motor
@@ -315,11 +373,12 @@ def set_servo_i(self, servo_id, data):
data (int): 0 - 254
"""
assert 0 <= data <= 254, "data must be between 0 and 254"
- self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
+ self.calibration_parameters(servo_id=servo_id)
self._mesg(ProtocolCode.MERCURY_DRAG_TECH_PAUSE, servo_id, data)
def get_servo_i(self, servo_id):
"""Reads the position loop I scale factor of the specified servo motor"""
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.GET_ERROR_DETECT_MODE, servo_id, has_reply=True)
def set_servo_cw(self, servo_id, data):
@@ -330,7 +389,7 @@ def set_servo_cw(self, servo_id, data):
data (int): 0 - 32
"""
assert 0 <= data <= 32, "data must be between 0 and 32"
- self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
+ self.calibration_parameters(servo_id=servo_id)
self._mesg(ProtocolCode.SET_SERVO_MOTOR_CLOCKWISE, servo_id, data)
def get_servo_cw(self, servo_id):
@@ -339,6 +398,7 @@ def get_servo_cw(self, servo_id):
Args:
servo_id (int): 0 - 254
"""
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.GET_SERVO_MOTOR_CLOCKWISE, servo_id, has_reply=True)
def set_servo_cww(self, servo_id, data):
@@ -349,7 +409,7 @@ def set_servo_cww(self, servo_id, data):
data (int): 0 - 32
"""
assert 0 <= data <= 32, "data must be between 0 and 32"
- self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id)
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.SET_SERVO_MOTOR_COUNTER_CLOCKWISE, servo_id, data)
def get_servo_cww(self, servo_id):
@@ -358,6 +418,7 @@ def get_servo_cww(self, servo_id):
Args:
servo_id (int): 0 - 254
"""
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.GET_SERVO_MOTOR_COUNTER_CLOCKWISE, servo_id, has_reply=True)
def set_servo_system_data(self, servo_id, addr, data, mode):
@@ -369,7 +430,13 @@ def set_servo_system_data(self, servo_id, addr, data, mode):
data (int): 0 - 4096
mode (int): 1 - data 1byte. 2 - data 2byte
"""
- self.calibration_parameters(class_name=self.__class__.__name__, servo_id=servo_id, servo_addr=addr)
+ if mode not in (1, 2):
+ raise ValueError('mode must be 1 or 2')
+
+ if data not in range(0, 4097):
+ raise ValueError('data must be between 0 and 4096')
+
+ self.calibration_parameters(servo_id=servo_id, servo_addr=addr)
self._mesg(ProtocolCode.SET_SERVO_MOTOR_CONFIG, servo_id, addr, [data], mode)
def get_servo_system_data(self, servo_id, addr, mode):
@@ -380,28 +447,35 @@ def get_servo_system_data(self, servo_id, addr, mode):
addr (int):
mode (int): 1 - data 1byte. 2 - data 2byte
"""
+ if mode not in (1, 2):
+ raise ValueError('mode must be 1 or 2')
+ self.calibration_parameters(servo_id=servo_id, servo_addr=addr)
return self._mesg(ProtocolCode.GET_SERVO_MOTOR_CONFIG, servo_id, addr, mode, has_reply=True)
- def set_master_out_io_state(self, io_number, status=1):
+ def set_master_out_io_state(self, pin_number, status=1):
"""Set the host I/O pin status
Args:
- io_number: 1 - 2
+ pin_number: M750(1 -6), C650(1-2)
status: 0/1; 0: low; 1: high. default: 1
"""
- self._mesg(ProtocolCode.SET_MASTER_PIN_STATUS, io_number, status)
+ if status not in (0, 1):
+ raise ValueError('status must be 0 or 1')
+ self.calibration_parameters(pin_number=pin_number)
+ self._mesg(ProtocolCode.SET_MASTER_PIN_STATUS, pin_number, status)
- def get_master_in_io_state(self, io_number):
+ def get_master_in_io_state(self, pin_number):
"""get the host I/O pin status
Args:
- io_number (int): 1 - 2
+ pin_number (int): M750(1 -6), C650(1-2)
Returns:
0 or 1. 1: high 0: low
"""
- return self._mesg(ProtocolCode.GET_MASTER_PIN_STATUS, io_number, has_reply=True)
+ self.calibration_parameters(pin_number=pin_number)
+ return self._mesg(ProtocolCode.GET_MASTER_PIN_STATUS, pin_number, has_reply=True)
def set_tool_out_io_state(self, io_number, status=1):
"""Set the Atom pin status
@@ -409,21 +483,24 @@ def set_tool_out_io_state(self, io_number, status=1):
Args:
io_number (int): 1 - 2
status: 0 or 1; 0: low; 1: high. default: 1
-
"""
+ self.calibration_parameters(master_pin=io_number, status=status)
self._mesg(ProtocolCode.SET_ATOM_PIN_STATUS, io_number, status)
- def get_tool_in_io_state(self, io_number):
+ def get_tool_in_io_state(self, pin):
"""Get the Atom pin status
Args:
- io_number (int): pin number
+ pin (int): pin
Returns:
0 or 1. 1: high 0: low
"""
- return self._mesg(ProtocolCode.GET_ATOM_PIN_STATUS, io_number, has_reply=True)
+ if pin not in (0, 1):
+ raise ValueError("pin must be 0 or 1")
+
+ return self._mesg(ProtocolCode.GET_ATOM_PIN_STATUS, pin, has_reply=True)
def set_tool_led_color(self, r, g, b):
"""Set the Atom LED color
diff --git a/pymycobot/myarmc.py b/pymycobot/myarmc.py
index 1c47f69..f6d37e0 100644
--- a/pymycobot/myarmc.py
+++ b/pymycobot/myarmc.py
@@ -1,14 +1,13 @@
# coding=utf-8
from __future__ import division
-
from pymycobot.common import ProtocolCode
from pymycobot.myarm_api import MyArmAPI
class MyArmC(MyArmAPI):
- def __init__(self, port, baudrate="1000000", timeout=0.1, debug=False):
+ def __init__(self, port, baudrate=1000000, timeout=0.1, debug=False):
super(MyArmC, self).__init__(port, baudrate, timeout, debug)
def is_tool_btn_clicked(self, mode=1):
@@ -21,6 +20,12 @@ def is_tool_btn_clicked(self, mode=1):
Returns:
list[int]: 0/1, 1: press, 0: no press
"""
+ if not isinstance(mode, int):
+ raise TypeError('mode must be int')
+
+ if mode not in [1, 2, 3, 254]:
+ raise ValueError('mode must be 1, 2, 3 or 254')
+
return self._mesg(ProtocolCode.GET_ATOM_PRESS_STATUS, mode, has_reply=True)
def get_joints_coord(self):
diff --git a/pymycobot/myarmm.py b/pymycobot/myarmm.py
index 10e1939..132d103 100644
--- a/pymycobot/myarmm.py
+++ b/pymycobot/myarmm.py
@@ -6,7 +6,7 @@
class MyArmM(MyArmAPI):
- def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
+ def __init__(self, port, baudrate="1000000", timeout=0.1, debug=False):
super(MyArmM, self).__init__(port, baudrate, timeout,debug)
def set_joint_angle(self, joint_id, angle, speed):
@@ -108,15 +108,11 @@ def get_robot_power_status(self):
return self._mesg(ProtocolCode.IS_POWER_ON, has_reply=True)
def set_robot_power_on(self):
- """Set the robot to power on
- Returns: (int) 1
- """
+ """Set the robot to power on"""
return self._mesg(ProtocolCode.POWER_ON, has_reply=True)
def set_robot_power_off(self):
- """Set the robot to power off
- Returns: (int) 1
- """
+ """Set the robot to power off"""
return self._mesg(ProtocolCode.POWER_OFF, has_reply=True)
def clear_robot_err(self):
diff --git a/pymycobot/myarmm_control.py b/pymycobot/myarmm_control.py
index 4f74415..0f402f2 100644
--- a/pymycobot/myarmm_control.py
+++ b/pymycobot/myarmm_control.py
@@ -1,5 +1,5 @@
# coding=utf-8
-
+import functools
import sys
import logging
import time
@@ -35,120 +35,134 @@ def setup_logging(debug: bool = False):
return logger
+def setup_serial_port(port, baudrate, timeout=0.1):
+ serial_api = serial.Serial(port, baudrate, timeout=timeout)
+ serial_api.rts = False
+ if not serial_api.is_open:
+ serial_api.open()
+ return serial_api
+
+
class MyArmMProcessor(DataProcessor):
def __init__(self, port, baudrate, timeout=0.1, debug=False):
-
- self._serial_port = serial.Serial()
- self._serial_port.port = port
- self._serial_port.baudrate = baudrate
- self._serial_port.timeout = timeout
- self._serial_port.rts = False
- self._serial_port.open()
+ super().__init__(debug)
self.lock = threading.Lock()
- self._version = sys.version_info[:2][0]
- self.log = setup_logging(debug)
- self.calibration_parameters = calibration_parameters
+ self._serial_port = setup_serial_port(port, baudrate, timeout)
+ self.calibration_parameters = functools.partial(calibration_parameters, class_name=self.__class__.__name__)
+ self._write = functools.partial(write, self, method=None)
+ self._read = functools.partial(read, self, _class=self.__class__.__name__)
- def _write(self, command):
- write(self, command, method=None)
-
- def _read(self, genre, command=None, _class=None, timeout=None):
- return read(self, genre, command, timeout, _class)
+ # def _write(self, command):
+ # write(self, command, method=None)
+ #
+ # def _read(self, genre, command=None, _class=None, timeout=None):
+ # return read(self, genre, command, timeout, _class)
+ @classmethod
+ def __is_return(cls, genre, command):
+ """
+ Check if the command is a return command.
+ """
+ return len(command) == 6 and command[3] == genre
+
def _mesg(self, genre, *args, **kwargs):
- real_command, has_reply = super(MyArmMProcessor, self)._mesg(genre, *args, **kwargs)
+ kwargs["has_reply"] = True
+ real_command, has_reply, _ = super(MyArmMProcessor, self)._mesg(genre, *args, **kwargs)
+ real_command = self._flatten(real_command)
+ self._write(real_command)
+
with self.lock:
- return self._res(real_command, has_reply, genre)
-
- def _res(self, real_command, has_reply, genre):
- self._write(self._flatten(real_command))
- if genre == ProtocolCode.STOP:
- has_reply = True
- if has_reply:
- data = self._read(genre, _class=self.__class__.__name__)
- if genre == ProtocolCode.SET_SSID_PWD:
- return None
+ return self._read_genre_result(genre)
+
+ def _read_genre_result(self, genre):
+ data = self._read(genre, timeout=None)
+ if genre == ProtocolCode.SET_SSID_PWD:
+ if len(data) == 5 and data[3] == genre:
+ return 1
+
+ if genre == ProtocolCode.SET_BASIC_OUTPUT:
+ if self.__is_return(genre, data):
+ return data[-2]
+
+ if genre == ProtocolCode.GET_ROBOT_STATUS:
+ res = []
+ valid_data = data[4:-1]
+ for header_i in range(0, len(valid_data), 2):
+ one = valid_data[header_i: header_i + 2]
+ res.append(self._decode_int16(one))
+ else:
res = self._process_received(data, genre)
- if res == []:
- return None
- if genre in [
- ProtocolCode.ROBOT_VERSION,
- ProtocolCode.GET_ROBOT_ID,
- ProtocolCode.IS_POWER_ON,
- ProtocolCode.IS_CONTROLLER_CONNECTED,
- ProtocolCode.IS_PAUSED, # TODO have bug: return b''
- ProtocolCode.IS_IN_POSITION,
- ProtocolCode.IS_MOVING,
- ProtocolCode.IS_SERVO_ENABLE,
- ProtocolCode.IS_ALL_SERVO_ENABLE,
- ProtocolCode.GET_SERVO_DATA,
- ProtocolCode.GET_DIGITAL_INPUT,
- ProtocolCode.GET_GRIPPER_VALUE,
- ProtocolCode.IS_GRIPPER_MOVING,
- ProtocolCode.GET_SPEED,
- ProtocolCode.GET_ENCODER,
- ProtocolCode.GET_BASIC_INPUT,
- ProtocolCode.GET_TOF_DISTANCE,
- ProtocolCode.GET_END_TYPE,
- ProtocolCode.GET_MOVEMENT_TYPE,
- ProtocolCode.GET_REFERENCE_FRAME,
- ProtocolCode.GET_FRESH_MODE,
- ProtocolCode.GET_GRIPPER_MODE,
- ProtocolCode.GET_ERROR_INFO,
- ProtocolCode.GET_COMMUNICATE_MODE,
- ProtocolCode.SET_COMMUNICATE_MODE,
- ProtocolCode.SetHTSGripperTorque,
- ProtocolCode.GetHTSGripperTorque,
- ProtocolCode.GetGripperProtectCurrent,
- ProtocolCode.InitGripper,
- ProtocolCode.SET_FOUR_PIECES_ZERO,
- ProtocolCode.STOP,
- ProtocolCode.IS_TOOL_BTN_CLICKED
- ]:
- return self._process_single(res)
- elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.SOLVE_INV_KINEMATICS]:
- return [self._int2angle(angle) for angle in res]
- elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
- if res:
- r = []
- for idx in range(3):
- r.append(self._int2coord(res[idx]))
- for idx in range(3, 6):
- r.append(self._int2angle(res[idx]))
- return r
+ if not res:
+ return -1
+
+ if genre in [
+ ProtocolCode.ROBOT_VERSION,
+ ProtocolCode.GET_ROBOT_ID,
+ ProtocolCode.IS_POWER_ON,
+ ProtocolCode.IS_CONTROLLER_CONNECTED,
+ ProtocolCode.IS_PAUSED,
+ ProtocolCode.IS_IN_POSITION,
+ ProtocolCode.IS_MOVING,
+ ProtocolCode.IS_SERVO_ENABLE,
+ ProtocolCode.IS_ALL_SERVO_ENABLE,
+ ProtocolCode.GET_SERVO_DATA,
+ ProtocolCode.GET_DIGITAL_INPUT,
+ ProtocolCode.GET_GRIPPER_VALUE,
+ ProtocolCode.IS_GRIPPER_MOVING,
+ ProtocolCode.GET_SPEED,
+ ProtocolCode.GET_ENCODER,
+ ProtocolCode.GET_BASIC_INPUT,
+ ProtocolCode.GET_TOF_DISTANCE,
+ ProtocolCode.GET_END_TYPE,
+ ProtocolCode.GET_MOVEMENT_TYPE,
+ ProtocolCode.GET_REFERENCE_FRAME,
+ ProtocolCode.GET_FRESH_MODE,
+ ProtocolCode.GET_GRIPPER_MODE,
+ ProtocolCode.GET_ERROR_INFO,
+ ProtocolCode.GET_COMMUNICATE_MODE,
+ ProtocolCode.SET_COMMUNICATE_MODE,
+ ProtocolCode.SetHTSGripperTorque,
+ ProtocolCode.GetHTSGripperTorque,
+ ProtocolCode.GetGripperProtectCurrent,
+ ProtocolCode.InitGripper,
+ ProtocolCode.SET_FOUR_PIECES_ZERO,
+ ProtocolCode.STOP,
+ ProtocolCode.IS_TOOL_BTN_CLICKED,
+ ProtocolCode.GET_ROBOT_TOOL_MODIFY_VERSION
+ ]:
+ return self._process_single(res)
+ elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.SOLVE_INV_KINEMATICS]:
+ return [self._int2angle(angle) for angle in res]
+ elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
+ r = []
+ for idx in range(3):
+ r.append(self._int2coord(res[idx]))
+ for idx in range(3, 6):
+ r.append(self._int2angle(res[idx]))
+ return r
+ elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
+ return [self._int2coord(angle) for angle in res]
+ elif genre in [ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE]:
+ return self._int2coord(res[0])
+ elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION,
+ ProtocolCode.GET_ATOM_VERSION]:
+ return self._int2coord(self._process_single(res))
+ elif genre == ProtocolCode.GET_ANGLES_COORDS:
+ r = []
+ for index in range(len(res)):
+ if index < 6:
+ r.append(self._int2angle(res[index]))
+ elif index < 9:
+ r.append(self._int2coord(res[index]))
else:
- return res
- elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
- return [self._int2coord(angle) for angle in res]
- elif genre in [ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE]:
- return self._int2coord(res[0])
- elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION]:
- return self._int2coord(self._process_single(res))
- elif genre == ProtocolCode.GET_ANGLES_COORDS:
- r = []
- for index in range(len(res)):
- if index < 6:
- r.append(self._int2angle(res[index]))
- elif index < 9:
- r.append(self._int2coord(res[index]))
- else:
- r.append(self._int2angle(res[index]))
- return r
- elif genre == ProtocolCode.GET_ROBOT_STATUS:
- for i in range(len(res)):
- if res[i] != 0:
- data = bin(res[i])[2:]
- res[i] = []
- while len(data) != 16:
- data = "0"+data
- for j in range(16):
- if data[j] != "0":
- res[i].append(15-j)
- return res
- else:
- return res
- return None
+ r.append(self._int2angle(res[index]))
+ return r
+ elif self.__is_return(genre, data):
+ print("11111111111111111111111")
+ return self._process_single(res)
+ else:
+ return res
class MyArmMControl(MyArmMProcessor):
@@ -159,19 +173,19 @@ def __init__(self, port, baudrate=115200, timeout=0.1, debug=False):
# System status
def get_robot_modify_version(self):
"""Get the bot correction version number"""
- return self._mesg(RobotProtocolCode.GET_ROBOT_MODIFY_VERSION, has_reply=True)
+ return self._mesg(RobotProtocolCode.GET_ROBOT_MODIFY_VERSION)
def get_robot_system_version(self):
"""Obtaining the Robot Firmware Version (Major and Minor Versions)"""
- return self._mesg(RobotProtocolCode.GET_ROBOT_SYSTEM_VERSION, has_reply=True)
+ return self._mesg(RobotProtocolCode.GET_ROBOT_SYSTEM_VERSION)
def get_robot_tool_modify_version(self):
"""Get the remediation version of the bot tool"""
- return self._mesg(ProtocolCode.GET_ROBOT_TOOL_MODIFY_VERSION, has_reply=True)
+ return self._mesg(ProtocolCode.GET_ROBOT_TOOL_MODIFY_VERSION)
def get_robot_tool_system_version(self):
"""Get the Robot Tool Firmware Version (End Atom)"""
- return self._mesg(RobotProtocolCode.GET_ROBOT_TOOL_SYSTEM_VERSION, has_reply=True)
+ return self._mesg(RobotProtocolCode.GET_ROBOT_TOOL_SYSTEM_VERSION)
def power_on(self):
"""The robotic arm turns on the power"""
@@ -189,7 +203,7 @@ def is_powered_on(self):
0 - power off
-1 - error data
"""
- return self._mesg(RobotProtocolCode.IS_POWERED_ON, has_reply=True)
+ return self._mesg(RobotProtocolCode.IS_POWERED_ON)
def release_all_servos(self, data=None):
"""The robot turns off the torque output
@@ -210,16 +224,16 @@ def set_fresh_mode(self, mode): # TODO 22-5-19 need test
1 - Always execute the latest command first.
0 - Execute instructions sequentially in the form of a queue.
"""
- self.calibration_parameters(class_name=self.__class__.__name__, mode=mode)
+ self.calibration_parameters(mode=mode)
return self._mesg(ProtocolCode.SET_FRESH_MODE, mode)
def get_fresh_mode(self):
"""Query sports mode"""
- return self._mesg(ProtocolCode.GET_FRESH_MODE, has_reply=True)
+ return self._mesg(ProtocolCode.GET_FRESH_MODE)
def get_robot_status(self):
"""Get robot status"""
- return self._mesg(ProtocolCode.GET_ROBOT_STATUS, has_reply=True)
+ return self._mesg(ProtocolCode.GET_ROBOT_STATUS)
def get_angles(self):
""" Get the angle of all joints.
@@ -227,7 +241,7 @@ def get_angles(self):
Return:
list: A float list of all angle.
"""
- return self._mesg(ProtocolCode.GET_ANGLES, has_reply=True)
+ return self._mesg(ProtocolCode.GET_ANGLES)
def write_angle(self, joint_id, degree, speed):
"""Send the angle of a joint to robot arm.
@@ -237,7 +251,7 @@ def write_angle(self, joint_id, degree, speed):
degree: (float) -150 ~ 150
speed : (int) 1 ~ 100
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, angle=degree, speed=speed)
+ self.calibration_parameters(joint_id=joint_id, angle=degree, speed=speed)
return self._mesg(ProtocolCode.SEND_ANGLE, joint_id, [self._angle2int(degree)], speed)
def write_angles(self, angles, speed):
@@ -247,6 +261,7 @@ def write_angles(self, angles, speed):
angles: (list) A float list of all angle.
speed : (int) 1 ~ 100
"""
+ self.calibration_parameters(angles=angles, speed=speed)
angles = [self._angle2int(angle) for angle in angles]
return self._mesg(ProtocolCode.SEND_ANGLES, angles, speed)
@@ -256,19 +271,19 @@ def get_coords(self):
Return:
list: A float list of all coordinates.
"""
- return self._mesg(ProtocolCode.GET_COORDS, has_reply=True)
+ return self._mesg(ProtocolCode.GET_COORDS)
- def write_coord(self, id, coord, speed):
+ def write_coord(self, coord_id, coord, speed):
"""Send the coordinates of a joint to robot arm.
Args:
- id: (int) 1 ~ 4
+ coord_id: (int) 1 ~ 6
coord: (float) -150 ~ 150
speed : (int) 1 ~ 100
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=id, coord=coord, speed=speed)
- value = self._coord2int(coord) if id <= 3 else self._angle2int(coord)
- return self._mesg(ProtocolCode.SEND_COORD, id, [value], speed)
+ self.calibration_parameters(coord_id=coord_id, coord=coord, speed=speed)
+ value = self._coord2int(coord) if coord_id <= 3 else self._angle2int(coord)
+ return self._mesg(ProtocolCode.SEND_COORD, coord_id, [value], speed)
def write_coords(self, coords, speed, mode=None):
"""Send the coordinates of all joints to robot arm.
@@ -278,7 +293,7 @@ def write_coords(self, coords, speed, mode=None):
speed : (int) 1 ~ 100
mode: (int) 0 - normal, 1 - low, 2 - high
"""
- self.calibration_parameters(class_name=self.__class__.__name__, coords=coords, speed=speed)
+ self.calibration_parameters(coords=coords, speed=speed)
coord_list = []
for idx in range(3):
coord_list.append(self._coord2int(coords[idx]))
@@ -289,33 +304,33 @@ def write_coords(self, coords, speed, mode=None):
else:
return self._mesg(ProtocolCode.SEND_COORDS, coord_list, speed)
- def is_in_position(self, data, id=0):
+ def is_in_position(self, data, mode=0):
"""Judge whether in the position.
Args:
data: A data list, angles or coords.
for myArm: angles len 7, coords len 6.
- id: 1 - coords, 0 - angles
+ mode: 1 - coords, 0 - angles
Return:
1 - True\n
0 - False\n
-1 - Error
"""
- if id == 1:
- self.calibration_parameters(class_name=self.__class__.__name__, coords=data)
+ if mode == 1:
+ self.calibration_parameters(coords=data)
data_list = []
for idx in range(3):
data_list.append(self._coord2int(data[idx]))
for idx in range(3, 6):
data_list.append(self._angle2int(data[idx]))
- elif id == 0:
- self.calibration_parameters(class_name=self.__class__.__name__, angles=data)
+ elif mode == 0:
+ self.calibration_parameters(angles=data)
data_list = [self._angle2int(i) for i in data]
else:
raise Exception("id is not right, please input 0 or 1")
- return self._mesg(ProtocolCode.IS_IN_POSITION, data_list, id, has_reply=True)
+ return self._mesg(ProtocolCode.IS_IN_POSITION, data_list, mode)
def is_moving(self):
"""Detect if the robot is moving
@@ -325,7 +340,7 @@ def is_moving(self):
1 - is moving
-1 - error data
"""
- return self._mesg(ProtocolCode.IS_MOVING, has_reply=True)
+ return self._mesg(ProtocolCode.IS_MOVING)
def jog_rpy(self, end_direction, direction, speed):
"""Rotate the end around a fixed axis in the base coordinate system
@@ -335,7 +350,8 @@ def jog_rpy(self, end_direction, direction, speed):
direction (int): 1 - forward rotation, 0 - reverse rotation
speed (int): 1 ~ 100
"""
- self.calibration_parameters(class_name=self.__class__.__name__, end_direction=end_direction)
+ self.calibration_parameters(direction=direction, speed=speed, end_direction=end_direction
+ )
return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed)
# JOG mode and operation
@@ -348,7 +364,7 @@ def jog_angle(self, joint_id, direction, speed):
direction: 0 - decrease, 1 - increase
speed: int (0 - 100)
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, direction=direction)
+ self.calibration_parameters(joint_id=joint_id, direction=direction, speed=speed)
return self._mesg(ProtocolCode.JOG_ANGLE, joint_id, direction, speed)
def jog_coord(self, coord_id, direction, speed):
@@ -359,19 +375,22 @@ def jog_coord(self, coord_id, direction, speed):
direction: 0 - decrease, 1 - increase
speed: int (1 - 100)
"""
- self.calibration_parameters(class_name=self.__class__.__name__, coord_id=coord_id, direction=direction)
+ self.calibration_parameters(coord_id=coord_id, direction=direction, speed=speed)
return self._mesg(ProtocolCode.JOG_COORD, coord_id, direction, speed)
def jog_increment(self, joint_id, increment, speed):
"""step mode
Args:
- joint_id:
- for myArm: Joint id 1 - 7.
- increment:
- speed: int (0 - 100)
+ joint_id(int):
+ for myArm: Joint id 1 - 6.
+ increment(int): incremental
+ speed(int): int (0 - 100)
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, speed=speed)
+ if not (isinstance(increment, int) or isinstance(increment, float)):
+ raise ValueError("increment must be int or float")
+
+ self.calibration_parameters(joint_id=joint_id, speed=speed)
return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
def pause(self):
@@ -386,7 +405,7 @@ def is_paused(self):
0 - not paused
-1 - error
"""
- return self._mesg(ProtocolCode.IS_PAUSED, has_reply=True)
+ return self._mesg(ProtocolCode.IS_PAUSED)
def resume(self):
"""Recovery movement"""
@@ -403,8 +422,8 @@ def get_encoder(self, joint_id):
joint_id: (int)
for myArm: Joint id 1 - 7.
"""
- self.calibration_parameters(class_name=self.__class__.__name__, encode_id=joint_id)
- return self._mesg(ProtocolCode.GET_ENCODER, joint_id, has_reply=True)
+ self.calibration_parameters(joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_ENCODER, joint_id)
def get_encoders(self):
"""Get the six joints of the manipulator
@@ -412,7 +431,7 @@ def get_encoders(self):
Returns:
The list of encoders
"""
- return self._mesg(ProtocolCode.GET_ENCODERS, has_reply=True)
+ return self._mesg(ProtocolCode.GET_ENCODERS)
# Running status and Settings
def get_speed(self):
@@ -421,7 +440,7 @@ def get_speed(self):
Returns:
int
"""
- return self._mesg(ProtocolCode.GET_SPEED, has_reply=True)
+ return self._mesg(ProtocolCode.GET_SPEED)
def set_speed(self, speed):
"""Set speed value
@@ -429,7 +448,7 @@ def set_speed(self, speed):
Args:
speed (int): 1 ~ 100
"""
- self.calibration_parameters(class_name=self.__class__.__name__, speed=speed)
+ self.calibration_parameters(speed=speed)
return self._mesg(ProtocolCode.SET_SPEED, speed)
def get_joint_min(self, joint_id):
@@ -442,8 +461,8 @@ def get_joint_min(self, joint_id):
Returns:
angle value(float)
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id)
- return self._mesg(ProtocolCode.GET_JOINT_MIN_ANGLE, joint_id, has_reply=True)
+ self.calibration_parameters(joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_JOINT_MIN_ANGLE, joint_id)
def get_joint_max(self, joint_id):
"""Gets the maximum movement angle of the specified joint
@@ -455,30 +474,30 @@ def get_joint_max(self, joint_id):
Return:
angle value(float)
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id)
- return self._mesg(ProtocolCode.GET_JOINT_MAX_ANGLE, joint_id, has_reply=True)
+ self.calibration_parameters(joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_JOINT_MAX_ANGLE, joint_id)
- def set_joint_max(self, id, angle):
+ def set_joint_max(self, joint_id, angle):
"""Set the joint maximum angle
Args:
- id: int.
+ joint_id: int.
for myArm: Joint id 1 - 7.
angle: 0 ~ 180
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=id, angle=angle)
- return self._mesg(ProtocolCode.SET_JOINT_MAX, id, angle)
+ self.calibration_parameters(joint_id=joint_id, angle=angle)
+ return self._mesg(ProtocolCode.SET_JOINT_MAX, joint_id, angle)
- def set_joint_min(self, id, angle):
+ def set_joint_min(self, joint_id, angle):
"""Set the joint minimum angle
Args:
- id: int.
+ joint_id: int.
for myArm: Joint id 1 - 7.
angle: 0 ~ 180
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=id, angle=angle)
- return self._mesg(ProtocolCode.SET_JOINT_MIN, id, angle)
+ self.calibration_parameters(joint_id=joint_id, angle=angle)
+ return self._mesg(ProtocolCode.SET_JOINT_MIN, joint_id, angle)
# Servo control
def is_servo_enable(self, servo_id):
@@ -493,8 +512,8 @@ def is_servo_enable(self, servo_id):
1 - enable
-1 - error
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id)
- return self._mesg(ProtocolCode.IS_SERVO_ENABLE, servo_id, has_reply=True)
+ self.calibration_parameters(servo_id=servo_id)
+ return self._mesg(ProtocolCode.IS_SERVO_ENABLE, servo_id)
def is_all_servo_enable(self):
"""Detect the connection status of all joints
@@ -504,7 +523,7 @@ def is_all_servo_enable(self):
1 - enable
-1 - error
"""
- return self._mesg(ProtocolCode.IS_ALL_SERVO_ENABLE, has_reply=True)
+ return self._mesg(ProtocolCode.IS_ALL_SERVO_ENABLE)
def set_servo_data(self, servo_id, data_id, value, mode=None):
"""Set the data parameters of the specified address of the steering gear
@@ -516,12 +535,14 @@ def set_servo_data(self, servo_id, data_id, value, mode=None):
value: 0 - 4096
mode: 0 - indicates that value is one byte(default), 1 - 1 represents a value of two bytes.
"""
+ if not isinstance(value, int):
+ raise TypeError("value must be int")
+
if mode is None:
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id, value=value)
+ self.calibration_parameters(servo_id=servo_id, servo_addr=data_id, value=value)
return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, value)
else:
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id, value=value,
- mode=mode)
+ self.calibration_parameters(servo_id=servo_id, servo_addr=data_id, value=value, mode=mode)
return self._mesg(ProtocolCode.SET_SERVO_DATA, servo_id, data_id, [value], mode)
def get_servo_data(self, servo_id, data_id, mode=None):
@@ -536,14 +557,11 @@ def get_servo_data(self, servo_id, data_id, mode=None):
values 0 - 4096
"""
if mode is not None:
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id, mode=mode)
- return self._mesg(
- ProtocolCode.GET_SERVO_DATA, servo_id, data_id, mode, has_reply=True
- )
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id, address=data_id)
- return self._mesg(
- ProtocolCode.GET_SERVO_DATA, servo_id, data_id, has_reply=True
- )
+ self.calibration_parameters(servo_id=servo_id, servo_addr=data_id, mode=mode)
+ return self._mesg(ProtocolCode.GET_SERVO_DATA, servo_id, data_id, mode)
+
+ self.calibration_parameters(servo_id=servo_id, servo_addr=data_id)
+ return self._mesg(ProtocolCode.GET_SERVO_DATA, servo_id, data_id)
def set_servo_calibration(self, servo_id):
"""The current position of the calibration joint actuator is the angle zero point,
@@ -552,7 +570,7 @@ def set_servo_calibration(self, servo_id):
Args:
servo_id: 1 ~ 8
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id)
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.SET_SERVO_CALIBRATION, servo_id)
def release_servo(self, servo_id, mode=None):
@@ -563,11 +581,11 @@ def release_servo(self, servo_id, mode=None):
mode: Default damping, set to 1, cancel damping
"""
if mode is None:
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id)
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id)
else:
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id)
+ self.calibration_parameters(servo_id=servo_id, mode=mode)
return self._mesg(ProtocolCode.RELEASE_SERVO, servo_id, mode)
def focus_servo(self, servo_id):
@@ -576,7 +594,7 @@ def focus_servo(self, servo_id):
Args:
servo_id: int 1 ~ 7
"""
- self.calibration_parameters(class_name=self.__class__.__name__, id=servo_id)
+ self.calibration_parameters(servo_id=servo_id)
return self._mesg(ProtocolCode.FOCUS_SERVO, servo_id)
def set_digital_output(self, pin_no, pin_signal):
@@ -586,73 +604,48 @@ def set_digital_output(self, pin_no, pin_signal):
pin_no (int):
pin_signal (int): 0 / 1
"""
- self.calibration_parameters(class_name=self.__class__.__name__, pin_signal=pin_signal)
+ self.calibration_parameters(pin_number=pin_no, pin_signal=pin_signal)
return self._mesg(ProtocolCode.SET_DIGITAL_OUTPUT, pin_no, pin_signal)
def get_digital_input(self, pin_no):
"""Get the terminal atom io status
Returns: int 0/1
"""
- return self._mesg(ProtocolCode.GET_DIGITAL_INPUT, pin_no, has_reply=True)
+ self.calibration_parameters(pin_number=pin_no)
+ return self._mesg(ProtocolCode.GET_DIGITAL_INPUT, pin_no)
def set_gripper_enabled(self):
"""Enable gripper"""
return self._mesg(RobotProtocolCode.SET_GRIPPER_ENABLED)
- def get_gripper_value(self, gripper_type=None):
+ def get_gripper_value(self):
"""Get the value of gripper.
- Args:
- gripper_type (int): default 1
- 1: Adaptive gripper
- 3: Parallel gripper
- 4: Flexible gripper
-
Return:
gripper value (int)
"""
- if gripper_type is None:
- return self._mesg(ProtocolCode.GET_GRIPPER_VALUE, has_reply=True)
- else:
- self.calibration_parameters(class_name=self.__class__.__name__, gripper_type=gripper_type)
- return self._mesg(ProtocolCode.GET_GRIPPER_VALUE, gripper_type, has_reply=True)
+ return self._mesg(ProtocolCode.GET_GRIPPER_VALUE)
- def set_gripper_state(self, flag, speed, _type_1=None):
+ def set_gripper_state(self, flag, speed):
"""Set gripper switch state
Args:
flag (int): 0 - open, 1 - close, 254 - release
speed (int): 1 ~ 100
- _type_1 (int): default 1
- 1 : Adaptive gripper. default to adaptive gripper
- 2 : 5 finger dexterous hand
- 3 : Parallel gripper, this parameter can be omitted
- 4 : Flexible gripper
- """
- if _type_1 is None:
- self.calibration_parameters(class_name=self.__class__.__name__, flag=flag, speed=speed)
- return self._mesg(ProtocolCode.SET_GRIPPER_STATE, flag, speed)
- else:
- self.calibration_parameters(class_name=self.__class__.__name__, flag=flag, speed=speed, _type_1=_type_1)
- return self._mesg(ProtocolCode.SET_GRIPPER_STATE, flag, speed, _type_1)
+ """
+ self.calibration_parameters(gripper_flag=flag, speed=speed)
+ return self._mesg(ProtocolCode.SET_GRIPPER_STATE, flag, speed)
- def set_gripper_value(self, gripper_value, speed, gripper_type=None):
+ def set_gripper_value(self, gripper_value, speed):
"""Set gripper value
Args:
gripper_value (int): 0 ~ 100
speed (int): 1 ~ 100
- gripper_type (int): default 1
- 1: Adaptive gripper
- 3: Parallel gripper, this parameter can be omitted
- 4: Flexible gripper
- """
- if gripper_type is not None:
- self.calibration_parameters(class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed,
- gripper_type=gripper_type)
- return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, gripper_value, speed, gripper_type)
- else:
- return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, gripper_value, speed)
+ """
+
+ self.calibration_parameters(gripper_value=gripper_value, speed=speed)
+ return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, gripper_value, speed)
def set_gripper_calibration(self):
"""Set the current position to zero, set current position value is `2048`."""
@@ -666,7 +659,7 @@ def is_gripper_moving(self):
1 - is moving
-1- error data
"""
- return self._mesg(ProtocolCode.IS_GRIPPER_MOVING, has_reply=True)
+ return self._mesg(ProtocolCode.IS_GRIPPER_MOVING)
# Atom IO
def set_led_color(self, r=0, g=0, b=0):
@@ -678,7 +671,7 @@ def set_led_color(self, r=0, g=0, b=0):
b (int): 0 ~ 255
"""
- self.calibration_parameters(class_name=self.__class__.__name__, rgb=[r, g, b])
+ self.calibration_parameters(rgb=(r, g, b))
return self._mesg(ProtocolCode.SET_COLOR, r, g, b)
def is_tool_btn_clicked(self):
@@ -689,7 +682,7 @@ def is_tool_btn_clicked(self):
1 - is clicked
-1- error data
"""
- return self._mesg(ProtocolCode.IS_TOOL_BTN_CLICKED, has_reply=True)
+ return self._mesg(ProtocolCode.IS_TOOL_BTN_CLICKED)
# Basic
def set_basic_output(self, pin_no, pin_signal):
@@ -699,7 +692,7 @@ def set_basic_output(self, pin_no, pin_signal):
pin_no: pin port number.
pin_signal: 0 / 1
"""
- self.calibration_parameters(class_name=self.__class__.__name__, pin_signal=pin_signal)
+ self.calibration_parameters(pin_signal=pin_signal, basic_pin_number=pin_no)
return self._mesg(ProtocolCode.SET_BASIC_OUTPUT, pin_no, pin_signal)
def get_basic_input(self, pin_no):
@@ -708,20 +701,20 @@ def get_basic_input(self, pin_no):
Args:
pin_no: (int) pin port number.
"""
- return self._mesg(ProtocolCode.GET_BASIC_INPUT, pin_no, has_reply=True)
+ self.calibration_parameters(basic_pin_number=pin_no)
+ return self._mesg(ProtocolCode.GET_BASIC_INPUT, pin_no)
- def set_ssid_pwd(self, account, password):
+ def set_ssid_pwd(self, account: str, password: str):
"""Change connected wi-fi.
Args:
account: (str) new wi-fi account.
password: (str) new wi-fi password.
"""
- self.calibration_parameters(class_name=self.__class__.__name__, account=account, password=password)
-
- self._mesg(ProtocolCode.SET_SSID_PWD) # 先发指令，再发设置的账号密码
+ self._mesg(ProtocolCode.SET_SSID_PWD)
time.sleep(0.02)
- return self._mesg(ProtocolCode.SET_SSID_PWD, account, password, has_reply=True)
+ self.calibration_parameters(account=account, password=password)
+ return self._mesg(ProtocolCode.SET_SSID_PWD, account, password)
def get_ssid_pwd(self):
"""Get connected wi-fi account and password.
@@ -729,7 +722,7 @@ def get_ssid_pwd(self):
Return:
(account, password)
"""
- return self._mesg(ProtocolCode.GET_SSID_PWD, has_reply=True)
+ return self._mesg(ProtocolCode.GET_SSID_PWD)
def set_server_port(self, port):
"""Change the connection port of the server.
@@ -737,6 +730,8 @@ def set_server_port(self, port):
Args:
port: (int) The new connection port of the server.
"""
+ if not isinstance(port, int):
+ raise TypeError("server port must be int")
return self._mesg(ProtocolCode.SET_SERVER_PORT, port)
def get_tof_distance(self):
@@ -745,7 +740,7 @@ def get_tof_distance(self):
Return:
(int) The unit is mm.
"""
- return self._mesg(ProtocolCode.GET_TOF_DISTANCE, has_reply=True)
+ return self._mesg(ProtocolCode.GET_TOF_DISTANCE)
def set_tool_reference(self, coords):
"""Set tool coordinate system
@@ -753,7 +748,7 @@ def set_tool_reference(self, coords):
Args:
coords: a list of coords value(List[float]), [x(mm), y, z, rx(angle), ry, rz]
"""
- self.calibration_parameters(class_name=self.__class__.__name__, coords=coords)
+ self.calibration_parameters(coords=coords)
coord_list = []
for idx in range(3):
coord_list.append(self._coord2int(coords[idx]))
@@ -763,7 +758,7 @@ def set_tool_reference(self, coords):
def get_tool_reference(self):
"""Get tool coordinate system """
- return self._mesg(ProtocolCode.GET_TOOL_REFERENCE, has_reply=True)
+ return self._mesg(ProtocolCode.GET_TOOL_REFERENCE)
def set_world_reference(self, coords):
"""Set the world coordinate system
@@ -771,7 +766,7 @@ def set_world_reference(self, coords):
Args:
coords: a list of coords value(List[float]), [x(mm), y, z, rx(angle), ry, rz]\n
"""
- self.calibration_parameters(class_name=self.__class__.__name__, coords=coords)
+ self.calibration_parameters(coords=coords)
coord_list = []
for idx in range(3):
coord_list.append(self._coord2int(coords[idx]))
@@ -781,7 +776,7 @@ def set_world_reference(self, coords):
def get_world_reference(self):
"""Get the world coordinate system"""
- return self._mesg(ProtocolCode.GET_WORLD_REFERENCE, has_reply=True)
+ return self._mesg(ProtocolCode.GET_WORLD_REFERENCE)
def set_reference_frame(self, rftype):
"""Set the base coordinate system
@@ -789,7 +784,7 @@ def set_reference_frame(self, rftype):
Args:
rftype: 0 - base 1 - tool.
"""
- self.calibration_parameters(class_name=self.__class__.__name__, rftype=rftype)
+ self.calibration_parameters(rftype=rftype)
return self._mesg(ProtocolCode.SET_REFERENCE_FRAME, rftype)
def get_reference_frame(self):
@@ -798,7 +793,7 @@ def get_reference_frame(self):
Returns:
0 - base 1 - tool.
"""
- return self._mesg(ProtocolCode.GET_REFERENCE_FRAME, has_reply=True)
+ return self._mesg(ProtocolCode.GET_REFERENCE_FRAME)
def set_movement_type(self, move_type):
"""Set movement type
@@ -806,7 +801,7 @@ def set_movement_type(self, move_type):
Args:
move_type: 1 - movel, 0 - moveJ
"""
- self.calibration_parameters(class_name=self.__class__.__name__, move_type=move_type)
+ self.calibration_parameters(move_type=move_type)
return self._mesg(ProtocolCode.SET_MOVEMENT_TYPE, move_type)
def get_movement_type(self):
@@ -815,16 +810,16 @@ def get_movement_type(self):
Returns:
1 - movel, 0 - moveJ
"""
- return self._mesg(ProtocolCode.GET_MOVEMENT_TYPE, has_reply=True)
+ return self._mesg(ProtocolCode.GET_MOVEMENT_TYPE)
- def set_end_type(self, end):
+ def set_end_type(self, mode):
"""Set end coordinate system
Args:
- end: int, 0 - flange, 1 - tool
+ mode: int, 0 - flange, 1 - tool
"""
- self.calibration_parameters(class_name=self.__class__.__name__, end=end)
- return self._mesg(ProtocolCode.SET_END_TYPE, end)
+ self.calibration_parameters(mode=mode)
+ return self._mesg(ProtocolCode.SET_END_TYPE, mode)
def get_end_type(self):
"""Get end coordinate system
@@ -832,7 +827,7 @@ def get_end_type(self):
Returns:
0 - flange, 1 - tool
"""
- return self._mesg(ProtocolCode.GET_END_TYPE, has_reply=True)
+ return self._mesg(ProtocolCode.GET_END_TYPE)
def get_plan_speed(self):
"""Get planning speed
@@ -840,7 +835,7 @@ def get_plan_speed(self):
Returns:
[movel planning speed, movej planning speed].
"""
- return self._mesg(ProtocolCode.GET_PLAN_SPEED, has_reply=True)
+ return self._mesg(ProtocolCode.GET_PLAN_SPEED)
def get_plan_acceleration(self):
"""Get planning acceleration
@@ -848,7 +843,7 @@ def get_plan_acceleration(self):
Returns:
[movel planning acceleration, movej planning acceleration].
"""
- return self._mesg(ProtocolCode.GET_PLAN_ACCELERATION, has_reply=True)
+ return self._mesg(ProtocolCode.GET_PLAN_ACCELERATION)
def set_plan_speed(self, speed, is_linear):
"""Set planning speed
@@ -857,6 +852,7 @@ def set_plan_speed(self, speed, is_linear):
speed (int): (1 ~ 100).
is_linear: 0 -> joint 1 -> straight line
"""
+ self.calibration_parameters(speed=speed, is_linear=is_linear)
return self._mesg(ProtocolCode.SET_PLAN_SPEED, speed, is_linear)
def set_plan_acceleration(self, acceleration, is_linear):
@@ -864,9 +860,9 @@ def set_plan_acceleration(self, acceleration, is_linear):
Args:
acceleration (int): (1 ~ 100).
- is_linear: 0 -> joint 1 -> straight line
+ is_linear(int): 0 -> joint 1 -> straight line
"""
- self.calibration_parameters(class_name=self.__class__.__name__, acceleration=acceleration, is_linear=is_linear)
+ self.calibration_parameters(speed=acceleration, is_linear=is_linear)
return self._mesg(ProtocolCode.SET_PLAN_ACCELERATION, acceleration, is_linear)
def get_servo_speeds(self):
@@ -874,14 +870,14 @@ def get_servo_speeds(self):
Returns:
speeds: list[float * 8] +- 3000 step/s
"""
- return self._mesg(ProtocolCode.GET_SERVO_SPEED, has_reply=True)
+ return self._mesg(ProtocolCode.GET_SERVO_SPEED)
def get_servo_currents(self):
"""Get the joint current
Returns:
currents: list[float * 8] 0 ~ 3250
"""
- return self._mesg(ProtocolCode.GET_SERVO_CURRENTS, has_reply=True)
+ return self._mesg(ProtocolCode.GET_SERVO_CURRENTS)
def get_servo_voltages(self):
"""Get the joint voltages
@@ -889,7 +885,7 @@ def get_servo_voltages(self):
Returns:
voltages: list[float] voltage 0 ~ 240
"""
- return self._mesg(ProtocolCode.GET_SERVO_VOLTAGES, has_reply=True)
+ return self._mesg(ProtocolCode.GET_SERVO_VOLTAGES)
def get_servo_status(self):
"""
@@ -899,7 +895,7 @@ def get_servo_status(self):
0 - normal,
other - error
"""
- return self._mesg(ProtocolCode.GET_SERVO_STATUS, has_reply=True)
+ return self._mesg(ProtocolCode.GET_SERVO_STATUS)
def get_servo_temps(self):
"""
@@ -907,7 +903,7 @@ def get_servo_temps(self):
Returns:
temperatures: list[float] 0 ~ 255
"""
- return self._mesg(ProtocolCode.GET_SERVO_TEMPS, has_reply=True)
+ return self._mesg(ProtocolCode.GET_SERVO_TEMPS)
def set_void_compensate(self, mode):
"""Set the virtual position compensation mode (
@@ -917,8 +913,6 @@ def set_void_compensate(self, mode):
Args:
mode (int): 0 - close, 1 - open
"""
- self.calibration_parameters(class_name = self.__class__.__name__, mode=mode)
+ self.calibration_parameters(mode=mode)
return self._mesg(ProtocolCode.SET_VOID_COMPENSATE, mode)
-
-
diff --git a/pymycobot/myarmsocket.py b/pymycobot/myarmsocket.py
index d8de66c..0792978 100644
--- a/pymycobot/myarmsocket.py
+++ b/pymycobot/myarmsocket.py
@@ -6,14 +6,15 @@
import socket
import threading
import logging
+import threading
from pymycobot.log import setup_logging
from pymycobot.generate import CommandGenerator
from pymycobot.common import ProtocolCode, write, read
from pymycobot.error import calibration_parameters
+from pymycobot.sms import sms_sts
-
-class MyArmSocket(CommandGenerator):
+class MyArmSocket(CommandGenerator, sms_sts):
"""MyArm Python API Serial communication class.
Note: Please use this class under the same network
@@ -62,6 +63,7 @@ def __init__(self, ip, netport=9000, debug=False):
self.SERVER_PORT = netport
self.sock = self.connect_socket()
self.lock = threading.Lock()
+ super(sms_sts, self).__init__(self._serial_port, 0)
def connect_socket(self):
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
@@ -80,7 +82,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyArmSocket, self)._mesg(genre, *args, **kwargs)
# [254,...,255]
with self.lock:
diff --git a/pymycobot/mybuddy.py b/pymycobot/mybuddy.py
index 84599e0..2b4b508 100644
--- a/pymycobot/mybuddy.py
+++ b/pymycobot/mybuddy.py
@@ -6,14 +6,15 @@
import logging
import threading
import struct
+import threading
from pymycobot.log import setup_logging
from pymycobot.Interface import MyBuddyCommandGenerator
from pymycobot.common import ProtocolCode, write, read
from pymycobot.error import calibration_parameters
+from pymycobot.sms import sms_sts
-
-class MyBuddy(MyBuddyCommandGenerator):
+class MyBuddy(MyBuddyCommandGenerator, sms_sts):
"""MyCobot Python API Serial communication class.
Supported methods:
@@ -65,6 +66,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
self._serial_port.rts = False
self._serial_port.open()
self.lock = threading.Lock()
+ super(sms_sts, self).__init__(self._serial_port, 0)
_write = write
@@ -134,7 +136,7 @@ def _mesg(self, genre, *args, **kwargs):
if has_reply:
data = self._read()
res = self._process_received(data, genre, arm=12)
- if res == []:
+ if res is None:
return None
if genre in [
ProtocolCode.ROBOT_VERSION,
diff --git a/pymycobot/mybuddybluetooth.py b/pymycobot/mybuddybluetooth.py
index 25035f1..87a7b89 100644
--- a/pymycobot/mybuddybluetooth.py
+++ b/pymycobot/mybuddybluetooth.py
@@ -6,6 +6,7 @@
from pymycobot.common import ProtocolCode, write, read
from pymycobot.error import calibration_parameters
from pymycobot.bluet import BluetoothConnection
+import threading
class MyBuddyBlueTooth(MyBuddyCommandGenerator):
@@ -53,81 +54,84 @@ def _mesg(self, genre, *args, **kwargs):
"""
real_command, has_reply = super(
MyBuddyBlueTooth, self)._mesg(genre, *args, **kwargs)
+
with self.lock:
data = self._write(self._flatten(real_command), "socket")
- if data:
- res = self._process_received(data, genre, arm=12)
- if res == []:
- return None
- if genre in [
- ProtocolCode.ROBOT_VERSION,
- ProtocolCode.SOFTWARE_VERSION,
- ProtocolCode.GET_ROBOT_ID,
- ProtocolCode.IS_POWER_ON,
- ProtocolCode.IS_CONTROLLER_CONNECTED,
- ProtocolCode.IS_PAUSED, # TODO have bug: return b''
- ProtocolCode.IS_IN_POSITION,
- ProtocolCode.IS_MOVING,
- ProtocolCode.IS_SERVO_ENABLE,
- ProtocolCode.IS_ALL_SERVO_ENABLE,
- ProtocolCode.GET_SERVO_DATA,
- ProtocolCode.GET_DIGITAL_INPUT,
- ProtocolCode.GET_GRIPPER_VALUE,
- ProtocolCode.IS_GRIPPER_MOVING,
- ProtocolCode.GET_SPEED,
- ProtocolCode.GET_ENCODER,
- ProtocolCode.GET_BASIC_INPUT,
- ProtocolCode.GET_TOF_DISTANCE,
- ProtocolCode.GET_END_TYPE,
- ProtocolCode.GET_MOVEMENT_TYPE,
- ProtocolCode.GET_REFERENCE_FRAME,
- ProtocolCode.GET_FRESH_MODE,
- ProtocolCode.SetHTSGripperTorque,
- ProtocolCode.GetHTSGripperTorque,
- ProtocolCode.GetGripperProtectCurrent,
- ProtocolCode.InitGripper,
- ProtocolCode.SET_FOUR_PIECES_ZERO
- # ProtocolCode.GET_SERVO_CURRENTS
- ]:
- return self._process_single(res)
- elif genre in [ProtocolCode.GET_ANGLES]:
- return [self._int2angle(angle) for angle in res]
- elif genre in [ProtocolCode.GET_ANGLE]:
- return self._int2angle(res[0]) if res else None
- elif genre in [ProtocolCode.GET_COORD]:
- if real_command[5] < 4:
- if real_command[2] == 3:
- return self._int2angle(res[0]) if res else None
- return self._int2coord(res[0]) if res else None
- else:
+ if data:
+ res = self._process_received(data, genre, arm=12)
+ if res == []:
+ return None
+ if genre in [
+ ProtocolCode.ROBOT_VERSION,
+ ProtocolCode.SOFTWARE_VERSION,
+ ProtocolCode.GET_ROBOT_ID,
+ ProtocolCode.IS_POWER_ON,
+ ProtocolCode.IS_CONTROLLER_CONNECTED,
+ ProtocolCode.IS_PAUSED, # TODO have bug: return b''
+ ProtocolCode.IS_IN_POSITION,
+ ProtocolCode.IS_MOVING,
+ ProtocolCode.IS_SERVO_ENABLE,
+ ProtocolCode.IS_ALL_SERVO_ENABLE,
+ ProtocolCode.GET_SERVO_DATA,
+ ProtocolCode.GET_DIGITAL_INPUT,
+ ProtocolCode.GET_GRIPPER_VALUE,
+ ProtocolCode.IS_GRIPPER_MOVING,
+ ProtocolCode.GET_SPEED,
+ ProtocolCode.GET_ENCODER,
+ ProtocolCode.GET_BASIC_INPUT,
+ ProtocolCode.GET_TOF_DISTANCE,
+ ProtocolCode.GET_END_TYPE,
+ ProtocolCode.GET_MOVEMENT_TYPE,
+ ProtocolCode.GET_REFERENCE_FRAME,
+ ProtocolCode.GET_FRESH_MODE,
+ ProtocolCode.SetHTSGripperTorque,
+ ProtocolCode.GetHTSGripperTorque,
+ ProtocolCode.GetGripperProtectCurrent,
+ ProtocolCode.InitGripper,
+ ProtocolCode.SET_FOUR_PIECES_ZERO
+ # ProtocolCode.GET_SERVO_CURRENTS
+ ]:
+ return self._process_single(res)
+ elif genre in [ProtocolCode.GET_ANGLES]:
+ return [self._int2angle(angle) for angle in res]
+ elif genre in [ProtocolCode.GET_ANGLE]:
+ return self._int2angle(res[0]) if res else None
+ elif genre in [ProtocolCode.GET_COORD]:
+ if real_command[5] < 4:
+ if real_command[2] == 3:
return self._int2angle(res[0]) if res else None
- elif genre in [ProtocolCode.GET_ALL_BASE_COORDS, ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE, ProtocolCode.GET_BASE_COORDS, ProtocolCode.GET_BASE_COORD, ProtocolCode.BASE_TO_SINGLE_COORDS]:
- if res:
- r = []
- for idx in range(3):
- r.append(self._int2coord(res[idx]))
- for idx in range(3, 6):
- r.append(self._int2angle(res[idx]))
- if len(res) == 12:
- r1 = []
- for idx in range(6, 9):
- r1.append(self._int2coord(res[idx]))
- for idx in range(9, 12):
- r1.append(self._int2angle(res[idx]))
- return [r, r1]
- return r
- else:
- return res
- elif genre in [ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE]:
- return self._int2coord(res[0])
- elif genre in [ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.COLLISION]:
- return [self._int2coord(angle) for angle in res]
+ return self._int2coord(res[0]) if res else None
+ else:
+ return self._int2angle(res[0]) if res else None
+ elif genre in [ProtocolCode.GET_ALL_BASE_COORDS, ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE, ProtocolCode.GET_BASE_COORDS, ProtocolCode.GET_BASE_COORD, ProtocolCode.BASE_TO_SINGLE_COORDS]:
+ if res:
+ r = []
+ for idx in range(3):
+ r.append(self._int2coord(res[idx]))
+ for idx in range(3, 6):
+ r.append(self._int2angle(res[idx]))
+ if len(res) == 12:
+ r1 = []
+ for idx in range(6, 9):
+ r1.append(self._int2coord(res[idx]))
+ for idx in range(9, 12):
+ r1.append(self._int2angle(res[idx]))
+ return [r, r1]
+ return r
else:
return res
- return None
+ elif genre in [ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE]:
+ return self._int2coord(res[0])
+ elif genre in [ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.COLLISION]:
+ return [self._int2coord(angle) for angle in res]
+ else:
+ return res
+ return None
def close(self):
self._write("close","socket")
self.sock.close()
+
+
\ No newline at end of file
diff --git a/pymycobot/mybuddyemoticon.py b/pymycobot/mybuddyemoticon.py
index f8d7343..bd83077 100644
--- a/pymycobot/mybuddyemoticon.py
+++ b/pymycobot/mybuddyemoticon.py
@@ -32,7 +32,7 @@ def file_path(self):
"""
return self.__file_path
- def add_file_path(self, path_time):
+ def add_file_path(self, path_time: list):
"""Add Playback File
Args:
@@ -40,7 +40,7 @@ def add_file_path(self, path_time):
"""
self.__file_path.append(path_time)
- def del_file_path(self, index):
+ def del_file_path(self, index: int):
"""Delete the element with the specified subscript in the playlist list
Args:
@@ -55,7 +55,7 @@ def window_size(self):
return self.__window_size
@window_size.setter
- def window_size(self, data):
+ def window_size(self, data: tuple):
"""Set playback window size"""
self.__window_size = data
diff --git a/pymycobot/mybuddysocket.py b/pymycobot/mybuddysocket.py
index 5c02c7f..040a95a 100644
--- a/pymycobot/mybuddysocket.py
+++ b/pymycobot/mybuddysocket.py
@@ -4,13 +4,14 @@
import time
import math
import socket
+import logging
import threading
from pymycobot.log import setup_logging
from pymycobot.Interface import MyBuddyCommandGenerator
from pymycobot.common import ProtocolCode, write, read
from pymycobot.error import calibration_parameters
-
+# from pymycobot.sms import sms_sts
class MyBuddySocket(MyBuddyCommandGenerator):
"""MyCobot Python API Serial communication class.
@@ -61,6 +62,20 @@ def __init__(self, ip, netport=9000):
self.rasp = False
self.sock = self.connect_socket()
self.lock = threading.Lock()
+ # super(sms_sts, self).__init__(self._serial_port, 0)
+
+ def connect(self, serialport="/dev/ttyAMA0", baudrate="1000000", timeout='0.1'):
+ """Connect the robot arm through the serial port and baud rate
+ Args:
+ serialport: (str) default /dev/ttyAMA0
+ baudrate: default 1000000
+ timeout: default 0.1
+
+ """
+ self.rasp = True
+ self._write(serialport, "socket")
+ self._write(baudrate, "socket")
+ self._write(timeout, "socket")
def connect_socket(self):
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
@@ -86,7 +101,7 @@ def _mesg(self, genre, *args, **kwargs):
if has_reply:
data = self._read(genre, 'socket')
res = self._process_received(data, genre, arm=12)
- if res == []:
+ if res is None:
return None
if genre in [
ProtocolCode.ROBOT_VERSION,
diff --git a/pymycobot/mycobot.py b/pymycobot/mycobot.py
index 7e82471..c276fc5 100755
--- a/pymycobot/mycobot.py
+++ b/pymycobot/mycobot.py
@@ -11,9 +11,10 @@
from pymycobot.public import PublicCommandGenerator
from pymycobot.common import ProtocolCode, write, read
from pymycobot.error import calibration_parameters
+from pymycobot.sms import sms_sts
-class MyCobot(CommandGenerator, PublicCommandGenerator):
+class MyCobot(CommandGenerator, PublicCommandGenerator, sms_sts):
"""MyCobot Python API Serial communication class.
Supported methods:
@@ -56,10 +57,8 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False, thread_loc
debug : whether show debug info
"""
super(MyCobot, self).__init__(debug)
+ print("Note: This class is no longer maintained since v3.6.0, please refer to the project documentation: https://github.com/elephantrobotics/pymycobot/blob/main/README.md")
self.calibration_parameters = calibration_parameters
- self.thread_lock = thread_lock
- if thread_lock:
- self.lock = threading.Lock()
import serial
self._serial_port = serial.Serial()
self._serial_port.port = port
@@ -67,9 +66,9 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False, thread_loc
self._serial_port.timeout = timeout
self._serial_port.rts = False
self._serial_port.open()
- print("Note: Starting from version v3.6.0, the MyCobot class will no longer be maintained. For new usage, please refer to the documentation: https://github.com/elephantrobotics/pymycobot/tree/main/docs")
time.sleep(1.5)
-
+ self.lock = threading.Lock()
+ super(sms_sts, self).__init__(self._serial_port, 0)
_write = write
_read = read
@@ -86,7 +85,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyCobot, self)._mesg(genre, *args, **kwargs)
if self.thread_lock:
with self.lock:
@@ -107,6 +106,8 @@ def _res(self, real_command, has_reply, genre):
if genre == ProtocolCode.SET_SSID_PWD:
return None
res = self._process_received(data, genre)
+ if res is None:
+ return None
if res is not None and isinstance(res, list) and len(res) == 1:
return res[0]
if genre in [
@@ -194,7 +195,7 @@ def send_radians(self, radians, speed):
for radian in radians]
return self._mesg(ProtocolCode.SEND_ANGLES, degrees, speed)
- def sync_send_angles(self, degrees, speed, timeout=15):
+ def sync_send_angles(self, degrees, speed, timeout=30):
"""Send the angle in synchronous state and return when the target point is reached
Args:
@@ -207,9 +208,9 @@ def sync_send_angles(self, degrees, speed, timeout=15):
while time.time() - t < timeout:
f = self.is_in_position(degrees, 0)
if f == 1:
- break
+ return 1
time.sleep(0.1)
- return self
+ return 0
def sync_send_coords(self, coords, speed, mode=0, timeout=15):
"""Send the coord in synchronous state and return when the target point is reached
@@ -224,9 +225,9 @@ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
self.send_coords(coords, speed, mode)
while time.time() - t < timeout:
if self.is_in_position(coords, 1) == 1:
- break
+ return 1
time.sleep(0.1)
- return self
+ return 0
# Basic for raspberry pi.
def gpio_init(self):
@@ -269,4 +270,21 @@ def set_acceleration(self, acc):
Args:
acc: int
"""
- return self._mesg(ProtocolCode.SET_ACCELERATION, acc)
\ No newline at end of file
+ return self._mesg(ProtocolCode.SET_ACCELERATION, acc)
+
+ def go_home(self):
+ return self.send_angles([0,0,0,0,0,0], 10)
+
+ def angles_to_coords(self, angles):
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True)
+
+ def solve_inv_kinematics(self, target_coords, current_angles):
+ angles = [self._angle2int(angle) for angle in current_angles]
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(target_coords[idx]))
+ for angle in target_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
diff --git a/pymycobot/mycobot280.py b/pymycobot/mycobot280.py
index 3d11f31..950874f 100644
--- a/pymycobot/mycobot280.py
+++ b/pymycobot/mycobot280.py
@@ -110,6 +110,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False, thread_loc
self._serial_port.timeout = timeout
self._serial_port.rts = False
self._serial_port.open()
+ time.sleep(1.5) # use to 280 AR
_write = write
_read = read
@@ -126,7 +127,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyCobot280, self)._mesg(genre, *args, **kwargs)
if self.thread_lock:
with self.lock:
@@ -135,23 +136,32 @@ def _mesg(self, genre, *args, **kwargs):
return self._res(real_command, has_reply, genre)
def _res(self, real_command, has_reply, genre):
- try_count = 0
- while try_count < 3:
+ if genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
self._write(self._flatten(real_command))
data = self._read(genre)
- if data is not None and data != b'':
- break
- try_count += 1
else:
- return -1
+ try_count = 0
+ while try_count < 3:
+ self._write(self._flatten(real_command))
+ data = self._read(genre)
+ if data is not None and data != b'':
+ break
+ try_count += 1
+ else:
+ return -1
if genre == ProtocolCode.SET_SSID_PWD:
- return None
+ return 1
res = self._process_received(data, genre)
+ if res is None:
+ return None
+ if genre in [ProtocolCode.SET_BASIC_OUTPUT]:
+ return 1
if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [ProtocolCode.GET_BASIC_VERSION,
ProtocolCode.GET_JOINT_MIN_ANGLE,
ProtocolCode.GET_JOINT_MAX_ANGLE,
ProtocolCode.SOFTWARE_VERSION]:
return res[0]
+
if genre in [
ProtocolCode.IS_POWER_ON,
ProtocolCode.IS_CONTROLLER_CONNECTED,
@@ -183,7 +193,7 @@ def _res(self, real_command, has_reply, genre):
ProtocolCode.SET_FOUR_PIECES_ZERO
]:
return self._process_single(res)
- elif genre in [ProtocolCode.GET_ANGLES]:
+ elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.SOLVE_INV_KINEMATICS]:
return [self._int2angle(angle) for angle in res]
elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
if res:
@@ -201,6 +211,8 @@ def _res(self, real_command, has_reply, genre):
return self._int2coord(res[0])
elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION]:
return self._int2coord(self._process_single(res))
+ elif genre in [ProtocolCode.GET_REBOOT_COUNT]:
+ return self._process_high_low_bytes(res)
elif genre == ProtocolCode.GET_ANGLES_COORDS:
r = []
for index in range(len(res)):
@@ -304,7 +316,7 @@ def sync_send_angles(self, degrees, speed, timeout=15):
Args:
degrees: a list of degree values(List[float]), length 6.
speed: (int) 0 ~ 100
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_angles(degrees, speed)
@@ -322,7 +334,7 @@ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
coords: a list of coord values(List[float])
speed: (int) 0 ~ 100
mode: (int): 0 - angular（default）, 1 - linear
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_coords(coords, speed, mode)
@@ -366,6 +378,29 @@ def jog_rpy(self, end_direction, direction, speed):
self.calibration_parameters(class_name=self.__class__.__name__, end_direction=end_direction, speed=speed)
return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed)
+ def jog_increment_angle(self, joint_id, increment, speed):
+ """ angle step mode
+
+ Args:
+ joint_id: int 1-6.
+ increment: Angle increment value
+ speed: int (0 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, speed=speed)
+ return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
+
+ def jog_increment_coord(self, id, increment, speed):
+ """coord step mode
+
+ Args:
+ id: axis id 1 - 6.
+ increment: Coord increment value
+ speed: int (1 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=id, speed=speed)
+ value = self._coord2int(increment) if id <= 3 else self._angle2int(increment)
+ return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, id, [value], speed)
+
def set_HTS_gripper_torque(self, torque):
"""Set new adaptive gripper torque
@@ -638,6 +673,157 @@ def get_end_type(self):
"""
return self._mesg(ProtocolCode.GET_END_TYPE, has_reply=True)
+ def angles_to_coords(self, angles):
+ """ Convert angles to coordinates
+
+ Args:
+ angles : A float list of all angle.
+
+ Return:
+ list: A float list of all coordinates.
+ """
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True)
+
+ def solve_inv_kinematics(self, target_coords, current_angles):
+ """ Convert target coordinates to angles
+
+ Args:
+ target_coords: A float list of all coordinates.
+ current_angles : A float list of all angle.
+
+ Return:
+ list: A float list of all angle.
+ """
+ angles = [self._angle2int(angle) for angle in current_angles]
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(target_coords[idx]))
+ for angle in target_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
+ def set_vision_mode(self, flag):
+ """Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode.
+ (Only for visual tracking function)
+
+ Args:
+ flag: 0/1; 0 - close; 1 - open
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag)
+ return self._mesg(ProtocolCode.SET_VISION_MODE, flag)
+
+ def is_torque_gripper(self):
+ """Whether it is a force-controlled gripper
+
+ Return:
+ 40 - Force control
+ 9 - Non-force control
+ """
+ return self.get_servo_data(7, 1)
+
+ def set_gripper_state(self, flag, speed, _type_1=None, is_torque=None):
+ """Set gripper switch state
+
+ Args:
+ flag (int): 0 - open, 1 - close, 254 - release
+ speed (int): 1 ~ 100
+ _type_1 (int): default 1
+ 1 : Adaptive gripper. default to adaptive gripper
+ 2 : 5 finger dexterous hand
+ 3 : Parallel gripper, this parameter can be omitted
+ 4 : Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag, speed=speed, _type_1=_type_1,
+ is_torque=is_torque)
+ args = [flag, speed]
+ if _type_1 is not None:
+ args.append(_type_1)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_STATE, *args)
+
+ def set_gripper_value(self, gripper_value, speed, gripper_type=None, is_torque=None):
+ """Set gripper value
+
+ Args:
+ gripper_value (int): 0 ~ 100
+ speed (int): 1 ~ 100
+ gripper_type (int): default 1
+ 1: Adaptive gripper
+ 3: Parallel gripper, this parameter can be omitted
+ 4: Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed,
+ gripper_type=gripper_type, is_torque=is_torque)
+ args = [gripper_value, speed]
+ if gripper_type is not None:
+ args.append(gripper_type)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, *args, has_reply=True)
+
+ def drag_start_record(self): # TODO need test 2024/11/15
+ """Start track recording
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_START_RECORD, has_reply=True)
+
+ def drag_end_record(self): # TODO need test 2024/11/15
+ """End track recording
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_END_RECORD, has_reply=True)
+
+ def drag_get_record_data(self): # TODO need test 2024/11/15
+ """Get the recorded track
+
+ Return:
+ List of potential values (encoder values) and operating speeds of each joint
+ eg: [J1_encoder, J1_run_speed,J2_encoder, J2_run_speed,J3_encoder, J3_run_speed,J4_encoder, J4_run_speed,J5_
+ encoder, J5_run_speed,J6_encoder, J6_run_speed]
+ """
+
+ return self._mesg(ProtocolCode.DRAG_GET_RECORD_DATA, has_reply=True)
+
+ def drag_get_record_len(self): # TODO need test 2024/11/15
+ """Get the total number of recorded points
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_GET_RECORD_LEN, has_reply=True)
+
+ def drag_clear_record_data(self): # TODO need test 2024/11/15
+ """Clear recording track
+
+ Return:
+ Recording queue length 0
+ """
+
+ return self._mesg(ProtocolCode.DRAG_CLEAR_RECORD_DATA, has_reply=True)
+
+ def get_reboot_count(self):
+ """Read reboot count
+
+ Return:
+ reboot count
+ """
+ return self._mesg(ProtocolCode.GET_REBOOT_COUNT, has_reply=True)
+
# Other
def wait(self, t):
time.sleep(t)
@@ -648,3 +834,6 @@ def close(self):
def open(self):
self._serial_port.open()
+
+ def go_home(self):
+ return self.send_angles([0,0,0,0,0,0], 10)
diff --git a/pymycobot/mycobot280rdkx5.py b/pymycobot/mycobot280rdkx5.py
new file mode 100644
index 0000000..e73b34d
--- /dev/null
+++ b/pymycobot/mycobot280rdkx5.py
@@ -0,0 +1,898 @@
+#!/usr/bin/env python
+# -*- coding: UTF-8 -*-
+from __future__ import division
+
+import functools
+import socket
+import time
+import threading
+import serial
+
+from pymycobot.generate import CommandGenerator
+from pymycobot.common import ProtocolCode, write, read
+from pymycobot.error import calibration_parameters
+
+
+def setup_serial_connect(port, baudrate, timeout):
+ serial_api = serial.Serial(port=port, baudrate=baudrate, timeout=timeout)
+ serial_api.rts = False
+ if not serial_api.is_open:
+ serial_api.open()
+ return serial_api
+
+
+def setup_socket_connect(host, port, timeout):
+ socket_api = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ socket_api.settimeout(timeout)
+ socket_api.connect((host, port))
+ return socket_api
+
+
+class GPIOProtocolCode:
+ SETUP_GPIO_MODE = 0xAA
+ SETUP_GPIO_STATE = 0xAB
+ SET_GPIO_OUTPUT = 0xAC
+ GET_GPIO_INPUT = 0xAD
+
+
+class MyCobot280RDKX5Api(CommandGenerator):
+
+ def __init__(self, debug=False, thread_lock=True):
+ super(MyCobot280RDKX5Api, self).__init__(debug)
+ self.calibration_parameters = functools.partial(calibration_parameters, class_name="MyCobot280")
+ self.thread_lock = thread_lock
+ self.lock = threading.Lock()
+
+ def _mesg(self, genre, *args, **kwargs):
+ """
+
+ Args:
+ genre: command type (Command)
+ *args: other data.
+ It is converted to octal by default.
+ If the data needs to be encapsulated into hexadecimal,
+ the array is used to include them. (Data cannot be nested)
+ **kwargs: support `has_reply`
+ has_reply: Whether there is a return value to accept.
+ """
+ real_command, has_reply, _ = super(MyCobot280RDKX5Api, self)._mesg(genre, *args, **kwargs)
+ if self.thread_lock:
+ with self.lock:
+ return self._res(real_command, has_reply, genre)
+ else:
+ return self._res(real_command, has_reply, genre)
+
+ def _res(self, real_command, has_reply, genre):
+ if genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
+ self._write(self._flatten(real_command))
+ data = self._read(genre)
+ else:
+ try_count = 0
+ while try_count < 3:
+ self._write(self._flatten(real_command))
+ data = self._read(genre)
+ if data is not None and data != b'':
+ break
+ try_count += 1
+ else:
+ return -1
+ if genre == ProtocolCode.SET_SSID_PWD:
+ return 1
+
+ if genre == ProtocolCode.GET_QUICK_INFO:
+ res = []
+ valid_data = data[4:-1]
+ for header_i in range(0, len(valid_data), 2):
+ if header_i < 26:
+ one = valid_data[header_i: header_i + 2]
+ res.append(self._decode_int16(one))
+ res.extend(valid_data[25:])
+ else:
+ res = self._process_received(data, genre)
+
+ if res is None:
+ return None
+ if genre in [ProtocolCode.SET_BASIC_OUTPUT]:
+ return 1
+ if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
+ ProtocolCode.GET_BASIC_VERSION, ProtocolCode.GET_JOINT_MIN_ANGLE, ProtocolCode.GET_JOINT_MAX_ANGLE,
+ ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION
+ ]:
+ return res[0]
+
+ if genre in [
+ ProtocolCode.IS_POWER_ON,
+ ProtocolCode.IS_CONTROLLER_CONNECTED,
+ ProtocolCode.IS_PAUSED, # TODO have bug: return b''
+ ProtocolCode.IS_IN_POSITION,
+ ProtocolCode.IS_MOVING,
+ ProtocolCode.IS_SERVO_ENABLE,
+ ProtocolCode.IS_ALL_SERVO_ENABLE,
+ ProtocolCode.GET_SERVO_DATA,
+ ProtocolCode.GET_DIGITAL_INPUT,
+ ProtocolCode.GET_GRIPPER_VALUE,
+ ProtocolCode.IS_GRIPPER_MOVING,
+ ProtocolCode.GET_SPEED,
+ ProtocolCode.GET_ENCODER,
+ ProtocolCode.GET_BASIC_INPUT,
+ ProtocolCode.GET_TOF_DISTANCE,
+ ProtocolCode.GET_END_TYPE,
+ ProtocolCode.GET_MOVEMENT_TYPE,
+ ProtocolCode.GET_REFERENCE_FRAME,
+ ProtocolCode.GET_FRESH_MODE,
+ ProtocolCode.GET_GRIPPER_MODE,
+ ProtocolCode.GET_ERROR_INFO,
+ ProtocolCode.GET_COMMUNICATE_MODE,
+ ProtocolCode.SET_COMMUNICATE_MODE,
+ ProtocolCode.SetHTSGripperTorque,
+ ProtocolCode.GetHTSGripperTorque,
+ ProtocolCode.GetGripperProtectCurrent,
+ ProtocolCode.InitGripper,
+ ProtocolCode.SET_FOUR_PIECES_ZERO
+ ]:
+ return self._process_single(res)
+ elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.SOLVE_INV_KINEMATICS]:
+ return [self._int2angle(angle) for angle in res]
+ elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
+ if res:
+ r = []
+ for idx in range(3):
+ r.append(self._int2coord(res[idx]))
+ for idx in range(3, 6):
+ r.append(self._int2angle(res[idx]))
+ return r
+ else:
+ return res
+ elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
+ return [self._int2coord(angle) for angle in res]
+ elif genre in [ProtocolCode.GET_JOINT_MAX_ANGLE, ProtocolCode.GET_JOINT_MIN_ANGLE]:
+ return self._int2coord(res[0])
+ elif genre in [
+ ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION
+ ]:
+ return self._int2coord(self._process_single(res))
+ elif genre in [ProtocolCode.GET_REBOOT_COUNT]:
+ return self._process_high_low_bytes(res)
+ elif genre in (ProtocolCode.GET_ANGLES_COORDS, ProtocolCode.GET_QUICK_INFO):
+ r = []
+ for index, el in enumerate(res):
+ if index < 6:
+ r.append(self._int2angle(el))
+ elif index < 9:
+ r.append(self._int2coord(el))
+ elif index < 12:
+ r.append(self._int2angle(el))
+ else:
+ r.append(el)
+ return r
+ else:
+ return res
+
+ def wait(self, t):
+ time.sleep(t)
+ return self
+
+
+class MyCobot280RDKX5CommandGenerator(MyCobot280RDKX5Api):
+
+ # System Status
+ def get_modify_version(self):
+ """get modify version"""
+ return self._mesg(ProtocolCode.ROBOT_VERSION)
+
+ def get_system_version(self):
+ """get system version"""
+ return self._mesg(ProtocolCode.SOFTWARE_VERSION)
+
+ def clear_queue(self):
+ """Clear the command queue"""
+ return self._mesg(ProtocolCode.CLEAR_COMMAND_QUEUE)
+
+ def async_or_sync(self):
+ """Set the command execution mode
+ Return:
+ 0: Asynchronous mode
+ 1: Synchronous mode
+ """
+ return self._mesg(ProtocolCode.CHECK_ASYNC_OR_SYNC)
+
+ def get_error_information(self):
+ """Obtaining robot error information
+
+ Return:
+ 0: No error message.
+ 1 ~ 6: The corresponding joint exceeds the limit position.
+ 16 ~ 19: Collision protection.
+ 32: Kinematics inverse solution has no solution.
+ 33 ~ 34: Linear motion has no adjacent solution.
+ """
+ return self._mesg(ProtocolCode.GET_ERROR_INFO, has_reply=True)
+
+ def clear_error_information(self):
+ """Clear robot error message"""
+ return self._mesg(ProtocolCode.CLEAR_ERROR_INFO, has_reply=True)
+
+ def power_on(self):
+ """Open communication with Atom."""
+ return self._mesg(ProtocolCode.POWER_ON)
+
+ def power_off(self):
+ """Close communication with Atom."""
+ return self._mesg(ProtocolCode.POWER_OFF)
+
+ def is_power_on(self):
+ """Adjust robot arm status
+
+ Return:
+ 1 - power on
+ 0 - power off
+ -1 - error data
+ """
+ return self._mesg(ProtocolCode.IS_POWER_ON, has_reply=True)
+
+ def release_all_servos(self, data=None):
+ """Relax all joints
+
+ Args:
+ data: 1 - Undamping (The default is damping)
+ """
+ if data is None:
+ return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS)
+ else:
+ return self._mesg(ProtocolCode.RELEASE_ALL_SERVOS, data)
+
+ def read_next_error(self):
+ """Robot Error Detection
+
+ Return:
+ list len 6.
+ 0 : No abnormality
+ 1 : Communication disconnected
+ 2 : Unstable communication
+ 3 : Servo abnormality
+ """
+ return self._mesg(ProtocolCode.READ_NEXT_ERROR, has_reply=True)
+
+ def set_fresh_mode(self, mode):
+ """Set command refresh mode
+
+ Args:
+ mode: int.
+ 1 - Always execute the latest command first.
+ 0 - Execute instructions sequentially in the form of a queue.
+ """
+ self.calibration_parameters(mode=mode)
+ return self._mesg(ProtocolCode.SET_FRESH_MODE, mode)
+
+ def get_fresh_mode(self):
+ """Query sports mode"""
+ return self._mesg(ProtocolCode.GET_FRESH_MODE, has_reply=True)
+
+ def set_vision_mode(self, flag):
+ """Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode.
+ (Only for visual tracking function)
+
+ Args:
+ flag: 0/1; 0 - close; 1 - open
+ """
+ self.calibration_parameters(flag=flag)
+ return self._mesg(ProtocolCode.SET_VISION_MODE, flag)
+
+ def sync_send_angles(self, degrees, speed, timeout=15):
+ """Send the angle in synchronous state and return when the target point is reached
+
+ Args:
+ degrees: a list of degree values(List[float]), length 6.
+ speed: (int) 0 ~ 100
+ timeout: default 15s.
+ """
+ t = time.time()
+ self.send_angles(degrees, speed)
+ while time.time() - t < timeout:
+ f = self.is_in_position(degrees, 0)
+ if f == 1:
+ break
+ time.sleep(0.1)
+ return 1
+
+ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
+ """Send the coord in synchronous state and return when the target point is reached
+
+ Args:
+ coords: a list of coord values(List[float])
+ speed: (int) 0 ~ 100
+ mode: (int): 0 - angular（default）, 1 - linear
+ timeout: default 15s.
+ """
+ t = time.time()
+ self.send_coords(coords, speed, mode)
+ while time.time() - t < timeout:
+ if self.is_in_position(coords, 1) == 1:
+ break
+ time.sleep(0.1)
+ return 1
+
+ def get_angles_coords(self):
+ """Get joint angles and coordinates"""
+ return self._mesg(ProtocolCode.GET_ANGLES_COORDS, has_reply=True)
+
+ def get_quick_move_message(self):
+ """Get the quick move message"""
+ return self._mesg(ProtocolCode.GET_QUICK_INFO, has_reply=True)
+
+ # JOG mode and operation
+ def write_angles_time_control(self, angles, step_time):
+ """Write the angle of a joint in time control mode
+ Args:
+ angles: Angle value
+ step_time: Time unit: 30ms, range(1 ~ 255)
+ """
+ if step_time not in range(1, 256):
+ raise ValueError("step_time must be in range(1 ~ 255)")
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.WRITE_ANGLE_TIME, angles, step_time)
+
+ def jog_rpy(self, end_direction, direction, speed):
+ """Rotate the end around a fixed axis in the base coordinate system
+
+ Args:
+ end_direction (int): Roll, Pitch, Yaw (1-3)
+ direction (int): 1 - forward rotation, 0 - reverse rotation
+ speed (int): 1 ~ 100
+ """
+ self.calibration_parameters(end_direction=end_direction, speed=speed)
+ return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed)
+
+ def jog_increment_angle(self, joint_id, increment, speed):
+ """ angle step mode
+
+ Args:
+ joint_id: int 1-6.
+ increment: Angle increment value
+ speed: int (0 - 100)
+ """
+ self.calibration_parameters(id=joint_id, speed=speed)
+ return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
+
+ def jog_increment_coord(self, axis, increment, speed):
+ """coord step mode
+
+ Args:
+ axis: axis id 1 - 6.
+ increment: Coord increment value
+ speed: int (1 - 100)
+ """
+ self.calibration_parameters(id=axis, speed=speed)
+ value = self._coord2int(increment) if axis <= 3 else self._angle2int(increment)
+ return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, axis, [value], speed)
+
+ def set_HTS_gripper_torque(self, torque):
+ """Set new adaptive gripper torque
+
+ Args:
+ torque (int): 150 ~ 980
+
+ Return:
+ 0: Set failed
+ 1: Set successful
+ """
+ self.calibration_parameters(torque=torque)
+ return self._mesg(ProtocolCode.SetHTSGripperTorque, [torque], has_reply=True)
+
+ def get_HTS_gripper_torque(self):
+ """Get gripper torque
+
+ Returns:
+ int: 150 ~ 980
+ """
+ return self._mesg(ProtocolCode.GetHTSGripperTorque, has_reply=True)
+
+ def get_gripper_protect_current(self):
+ """Get the gripper protection current
+
+ Returns:
+ int: 1 ~ 500
+ """
+ return self._mesg(ProtocolCode.GetGripperProtectCurrent, has_reply=True)
+
+ def init_gripper(self):
+ """Initialize gripper
+
+ Returns:
+ int: 0 or 1 (1 - success)
+ """
+ return self._mesg(ProtocolCode.InitGripper, has_reply=True)
+
+ def set_gripper_protect_current(self, current):
+ """Set the gripper protection current
+
+ Args:
+ current (int): 1 ~ 500
+ """
+ self.calibration_parameters(current=current)
+
+ return self._mesg(ProtocolCode.SetGripperProtectCurrent, [current])
+
+ # Atom IO
+ def set_pin_mode(self, pin_no, pin_mode):
+ """Set the state mode of the specified pin in atom.
+
+ Args:
+ pin_no (int): pin.
+ pin_mode (int): 0 - input, 1 - output, 2 - input pull up
+ """
+ self.calibration_parameters(pin_mode=pin_mode)
+ return self._mesg(ProtocolCode.SET_PIN_MODE, pin_no, pin_mode)
+
+ def get_gripper_value(self, gripper_type=None):
+ """Get the value of gripper.
+
+ Args:
+ gripper_type (int): default 1
+ 1: Adaptive gripper
+ 3: Parallel gripper
+ 4: Flexible gripper
+
+ Return:
+ gripper value (int)
+ """
+ if gripper_type is None:
+ return self._mesg(ProtocolCode.GET_GRIPPER_VALUE, has_reply=True)
+ else:
+ self.calibration_parameters(gripper_type=gripper_type)
+ return self._mesg(ProtocolCode.GET_GRIPPER_VALUE, gripper_type, has_reply=True)
+
+ def is_gripper_moving(self):
+ """Judge whether the gripper is moving or not
+
+ Returns:
+ 0 - not moving
+ 1 - is moving
+ -1- error data
+ """
+ return self._mesg(ProtocolCode.IS_GRIPPER_MOVING, has_reply=True)
+
+ def get_tool_system_version(self):
+ """
+ Read the terminal primary and minor version numbers
+ """
+ return self._mesg(ProtocolCode.GET_ATOM_VERSION, has_reply=True)
+
+ def get_tool_modify_version(self):
+ """
+ Read the terminal modified version number
+ """
+ return self._mesg(ProtocolCode.OVER_LIMIT_RETURN_ZERO, has_reply=True)
+
+ def is_tool_connected(self):
+ """Check the end connection status"""
+ return self._mesg(ProtocolCode.IS_CONTROLLER_CONNECTED, has_reply=True)
+
+ def is_tool_button_click(self):
+ """Check whether the button on the end is pressed"""
+ return self._mesg(ProtocolCode.IS_CONTROLLER_BUTTON_PRESSED, has_reply=True)
+
+ def set_pwm_output(self, channel, frequency, pin_val):
+ """ PWM control
+
+ Args:
+ channel (int): IO number.
+ frequency (int): clock frequency
+ pin_val (int): Duty cycle 0 ~ 256; 128 means 50%
+ """
+ return self._mesg(ProtocolCode.SET_PWM_OUTPUT, channel, [frequency], pin_val)
+
+ # communication mode
+ def set_transponder_mode(self, mode):
+ """Set basic communication mode
+
+ Args:
+ mode: 0 - Turn off transparent transmission，1 - Open transparent transmission
+ """
+ self.calibration_parameters(mode=mode)
+ return self._mesg(ProtocolCode.SET_COMMUNICATE_MODE, mode, has_reply=True)
+
+ def get_transponder_mode(self):
+ return self._mesg(ProtocolCode.GET_COMMUNICATE_MODE, has_reply=True)
+
+ # g9 servo
+ def move_round(self):
+ """Drive the 9g steering gear clockwise for one revolution
+ """
+ return self._mesg(ProtocolCode.move_round)
+
+ def set_four_pieces_zero(self):
+ """Set the zero position of the four-piece motor
+
+ Returns:
+ int: 0 or 1 (1 - success)
+ """
+ return self._mesg(ProtocolCode.SET_FOUR_PIECES_ZERO, has_reply=True)
+
+ # Running Status and Settings
+ def set_joint_max(self, id, angle):
+ """Set the joint maximum angle
+
+ Args:
+ id: int.
+ Joint id 1 - 6
+ for gripper: Joint id 7
+ angle: 0 ~ 180
+ """
+ self.calibration_parameters(id=id, angle=angle)
+ return self._mesg(ProtocolCode.SET_JOINT_MAX, id, angle)
+
+ def set_joint_min(self, id, angle):
+ """Set the joint minimum angle
+
+ Args:
+ id: int.
+ Joint id 1 - 6
+ for gripper: Joint id 7
+ angle: 0 ~ 180
+ """
+ self.calibration_parameters(id=id, angle=angle)
+ return self._mesg(ProtocolCode.SET_JOINT_MIN, id, angle)
+
+ # servo state value
+ def get_servo_speeds(self):
+ """Get joint speed
+
+ Return:
+ A list unit step/s
+ """
+ return self._mesg(ProtocolCode.GET_SERVO_SPEED, has_reply=True)
+
+ def get_servo_currents(self):
+ """Get all joint current
+
+ Return:
+ A list unit mA
+ """
+ return self._mesg(ProtocolCode.GET_SERVO_CURRENTS, has_reply=True)
+
+ def get_servo_voltages(self):
+ """Get joint voltages
+
+ Return:
+ A list volts < 24 V
+ """
+ return self._mesg(ProtocolCode.GET_SERVO_VOLTAGES, has_reply=True)
+
+ def get_servo_status(self):
+ """Get joint status
+
+ Return:
+ [voltage, sensor, temperature, current, angle, overload], a value of 0 means no error, a value of 1 indicates an error
+ """
+ return self._mesg(ProtocolCode.GET_SERVO_STATUS, has_reply=True)
+
+ def get_servo_temps(self):
+ """Get joint temperature
+
+ Return:
+ A list unit ℃
+ """
+ return self._mesg(ProtocolCode.GET_SERVO_TEMPS, has_reply=True)
+
+ # Coordinate transformation
+ def set_tool_reference(self, coords):
+ """Set tool coordinate system
+
+ Args:
+ coords: a list of coords value(List[float])
+ [x(mm), y, z, rx(angle), ry, rz]
+ """
+ self.calibration_parameters(coords=coords)
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(coords[idx]))
+ for angle in coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SET_TOOL_REFERENCE, coord_list)
+
+ def get_tool_reference(self):
+ """Get tool coordinate system """
+ return self._mesg(ProtocolCode.GET_TOOL_REFERENCE, has_reply=True)
+
+ def set_world_reference(self, coords):
+ """Set the world coordinate system
+
+ Args:
+ coords: a list of coords value(List[float])
+ [x(mm), y, z, rx(angle), ry, rz]\n
+ """
+ self.calibration_parameters(coords=coords)
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(coords[idx]))
+ for angle in coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SET_WORLD_REFERENCE, coord_list)
+
+ def get_world_reference(self):
+ """Get the world coordinate system"""
+ return self._mesg(ProtocolCode.GET_WORLD_REFERENCE, has_reply=True)
+
+ def set_reference_frame(self, rftype):
+ """Set the base coordinate system
+
+ Args:
+ rftype: 0 - base 1 - tool.
+ """
+ self.calibration_parameters(rftype=rftype)
+ return self._mesg(ProtocolCode.SET_REFERENCE_FRAME, rftype)
+
+ def get_reference_frame(self):
+ """Get the base coordinate system
+
+ Return:
+ 0 - base 1 - tool.
+ """
+ return self._mesg(ProtocolCode.GET_REFERENCE_FRAME, has_reply=True)
+
+ def set_movement_type(self, move_type):
+ """Set movement type
+
+ Args:
+ move_type: 1 - movel, 0 - moveJ
+ """
+ self.calibration_parameters(move_type=move_type)
+ return self._mesg(ProtocolCode.SET_MOVEMENT_TYPE, move_type)
+
+ def get_movement_type(self):
+ """Get movement type
+
+ Return:
+ 1 - movel, 0 - moveJ
+ """
+ return self._mesg(ProtocolCode.GET_MOVEMENT_TYPE, has_reply=True)
+
+ def set_end_type(self, end):
+ """Set end coordinate system
+
+ Args:
+ end: int
+ 0 - flange, 1 - tool
+ """
+ self.calibration_parameters(end=end)
+ return self._mesg(ProtocolCode.SET_END_TYPE, end)
+
+ def get_end_type(self):
+ """Get end coordinate system
+
+ Return:
+ 0 - flange, 1 - tool
+ """
+ return self._mesg(ProtocolCode.GET_END_TYPE, has_reply=True)
+
+ def angles_to_coords(self, angles):
+ """ Convert angles to coordinates
+
+ Args:
+ angles : A float list of all angle.
+
+ Return:
+ list: A float list of all coordinates.
+ """
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True)
+
+ def solve_inv_kinematics(self, target_coords, current_angles):
+ """ Convert target coordinates to angles
+
+ Args:
+ target_coords: A float list of all coordinates.
+ current_angles : A float list of all angle.
+
+ Return:
+ list: A float list of all angle.
+ """
+ angles = [self._angle2int(angle) for angle in current_angles]
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(target_coords[idx]))
+ for angle in target_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
+ def is_torque_gripper(self):
+ """Whether it is a force-controlled gripper
+
+ Return:
+ 40 - Force control
+ 9 - Non-force control
+ """
+ return self.get_servo_data(7, 1)
+
+ def set_gripper_state(self, flag, speed, _type_1=None, is_torque=None):
+ """Set gripper switch state
+
+ Args:
+ flag (int): 0 - open, 1 - close, 254 - release
+ speed (int): 1 ~ 100
+ _type_1 (int): default 1
+ 1 : Adaptive gripper. default to adaptive gripper
+ 2 : 5 finger dexterous hand
+ 3 : Parallel gripper, this parameter can be omitted
+ 4 : Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(flag=flag, speed=speed, _type_1=_type_1, is_torque=is_torque)
+ args = [flag, speed]
+ if _type_1 is not None:
+ args.append(_type_1)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_STATE, *args)
+
+ def set_gripper_value(self, gripper_value, speed, gripper_type=None, is_torque=None):
+ """Set gripper value
+
+ Args:
+ gripper_value (int): 0 ~ 100
+ speed (int): 1 ~ 100
+ gripper_type (int): default 1
+ 1: Adaptive gripper
+ 3: Parallel gripper, this parameter can be omitted
+ 4: Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(gripper_value=gripper_value, speed=speed,
+ gripper_type=gripper_type, is_torque=is_torque)
+ args = [gripper_value, speed]
+ if gripper_type is not None:
+ args.append(gripper_type)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, *args, has_reply=True)
+
+ def drag_start_record(self):
+ """Start track recording
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_START_RECORD, has_reply=True)
+
+ def drag_end_record(self):
+ """End track recording
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_END_RECORD, has_reply=True)
+
+ def drag_get_record_data(self):
+ """Get the recorded track
+
+ Return:
+ List of potential values (encoder values) and operating speeds of each joint
+ eg: [J1_encoder, J1_run_speed,J2_encoder, J2_run_speed,J3_encoder, J3_run_speed,J4_encoder, J4_run_speed,J5_
+ encoder, J5_run_speed,J6_encoder, J6_run_speed]
+ """
+
+ return self._mesg(ProtocolCode.DRAG_GET_RECORD_DATA, has_reply=True)
+
+ def drag_get_record_len(self):
+ """Get the total number of recorded points
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_GET_RECORD_LEN, has_reply=True)
+
+ def drag_clear_record_data(self):
+ """Clear recording track
+
+ Return:
+ Recording queue length 0
+ """
+
+ return self._mesg(ProtocolCode.DRAG_CLEAR_RECORD_DATA, has_reply=True)
+
+
+class MyCobot280RDKX5(MyCobot280RDKX5CommandGenerator):
+
+ def __init__(self, port, baudrate=100_0000, timeout=0.1, debug=False, thread_lock=True):
+ """
+ Args:
+ port : port string
+ baudrate : baud rate int, default 100_0000
+ timeout : default 0.1
+ debug : whether show debug info
+ """
+ super().__init__(debug, thread_lock)
+ self._serial_port = setup_serial_connect(port=port, baudrate=baudrate, timeout=timeout)
+ self._write = functools.partial(write, self)
+ self._read = functools.partial(read, self)
+
+ def close(self):
+ self._serial_port.close()
+
+ def open(self):
+ self._serial_port.open()
+
+
+class MyCobot280RDKX5Socket(MyCobot280RDKX5CommandGenerator):
+ """MyCobot 280 RDK X5 Socket Control Class
+
+ server file: https://github.com/elephantrobotics/pymycobot/demo/Server_280_RDK_X5.py
+ """
+ def __init__(self, ip, port=30002, timeout=0.1, debug=False, thread_lock=True):
+ super().__init__(debug, thread_lock)
+ self.sock = setup_socket_connect(ip, port, timeout)
+ self._write = functools.partial(write, self, method="socket")
+ self._read = functools.partial(read, self, method="socket")
+
+ def set_gpio_mode(self, mode):
+ """Set pin coding method
+ Args:
+ mode: (int) 0 - BCM, 1 - BOARD
+
+ returns:
+ (int) 1 - success, 255 - error
+ """
+ if mode not in (0, 1):
+ raise ValueError("mode must be 0 or 1")
+ return self._mesg(GPIOProtocolCode.SETUP_GPIO_MODE, mode)
+
+ def setup_gpio_state(self, pin_no, mode, initial=1):
+ """Set the pin as input or output
+ Args:
+ pin_no: (int) pin id
+ mode: (int) 0 - input, 1 - output
+ initial: (int) 0 - low, 1 - high
+ returns:
+ (int) 1 - success, 255 - error
+ """
+ if mode not in (0, 1):
+ raise ValueError("mode must be 0 or 1")
+
+ if initial not in (0, 1):
+ raise ValueError("initial must be 0 or 1")
+
+ return self._mesg(GPIOProtocolCode.SETUP_GPIO_STATE, pin_no, mode, initial)
+
+ def set_gpio_output(self, pin_no, state):
+ """Set the pin to high or low level
+ Args:
+ pin_no: (int) pin id.
+ state: (int) 0 - low, 1 - high
+ returns:
+ (int) 1 - success, 255 - error
+ """
+ return self._mesg(GPIOProtocolCode.SET_GPIO_OUTPUT, pin_no, state)
+
+ def get_gpio_input(self, pin_no):
+ """Get pin level status.
+ Args:
+ pin_no: (int) pin id.
+ Returns:
+ (int) 0 - low, 1 - high, 255 - error
+ """
+ return self._mesg(GPIOProtocolCode.GET_GPIO_INPUT, pin_no, has_reply=True)
+
+ def close(self):
+ self.sock.close()
+
+
+def main():
+ mc_sock = MyCobot280RDKX5Socket('192.168.1.246', port=30002, debug=True)
+ # print(mc_sock.send_angle(1, 100, 50))
+ # print(mc_sock.get_quick_move_message())
+ print(mc_sock.set_gpio_mode(0))
+ print(mc_sock.setup_gpio_state(5, 1, initial=1))
+ print(mc_sock.set_gpio_output(5, 0))
+ # print(mc_sock.get_gpio_input(5))
+
+
+if __name__ == '__main__':
+ main()
diff --git a/pymycobot/mycobot280socket.py b/pymycobot/mycobot280socket.py
index 1760517..01c1947 100644
--- a/pymycobot/mycobot280socket.py
+++ b/pymycobot/mycobot280socket.py
@@ -103,6 +103,7 @@ def __init__(self, ip, netport=9000, debug=False):
self.SERVER_PORT = netport
self.sock = self.connect_socket()
self.lock = threading.Lock()
+ time.sleep(1.5) # use to 280 AR
def connect_socket(self):
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
@@ -121,26 +122,32 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyCobot280Socket, self)._mesg(genre, *args, **kwargs)
# [254,...,255]
with self.lock:
# data = self._write(self._flatten(real_command), "socket")
# # if has_reply:
# data = self._read(genre, method='socket')
- try_count = 0
- while try_count < 3:
+ if genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
self._write(self._flatten(real_command), "socket")
data = self._read(genre, method='socket')
- if data is not None and data != b'':
- break
- try_count += 1
else:
- return -1
+ try_count = 0
+ while try_count < 3:
+ self._write(self._flatten(real_command), "socket")
+ data = self._read(genre, method='socket')
+ if data is not None and data != b'':
+ break
+ try_count += 1
+ else:
+ return -1
if genre == ProtocolCode.SET_SSID_PWD:
- return None
+ return 1
+ if genre in [ProtocolCode.SET_BASIC_OUTPUT]:
+ return 1
res = self._process_received(data, genre)
- if res == []:
+ if res is None:
return None
elif res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
ProtocolCode.GET_BASIC_VERSION,
@@ -181,7 +188,7 @@ def _mesg(self, genre, *args, **kwargs):
ProtocolCode.SET_FOUR_PIECES_ZERO
]:
return self._process_single(res)
- elif genre in [ProtocolCode.GET_ANGLES]:
+ elif genre in [ProtocolCode.GET_ANGLES, ProtocolCode.SOLVE_INV_KINEMATICS]:
return [self._int2angle(angle) for angle in res]
elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
if res:
@@ -200,6 +207,8 @@ def _mesg(self, genre, *args, **kwargs):
elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION,
ProtocolCode.GET_ATOM_VERSION]:
return self._int2coord(self._process_single(res))
+ elif genre in [ProtocolCode.GET_REBOOT_COUNT]:
+ return self._process_high_low_bytes(res)
elif genre == ProtocolCode.GET_ANGLES_COORDS:
r = []
for index in range(len(res)):
@@ -303,7 +312,7 @@ def sync_send_angles(self, degrees, speed, timeout=15):
Args:
degrees: a list of degree values(List[float]), length 6.
speed: (int) 0 ~ 100
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_angles(degrees, speed)
@@ -321,7 +330,7 @@ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
coords: a list of coord values(List[float])
speed: (int) 0 ~ 100
mode: (int): 0 - angular（default）, 1 - linear
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_coords(coords, speed, mode)
@@ -365,6 +374,29 @@ def jog_rpy(self, end_direction, direction, speed):
self.calibration_parameters(class_name=self.__class__.__name__, end_direction=end_direction, speed=speed)
return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed)
+ def jog_increment_angle(self, joint_id, increment, speed):
+ """ angle step mode
+
+ Args:
+ joint_id: int 1-6.
+ increment: Angle increment value
+ speed: int (0 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, speed=speed)
+ return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
+
+ def jog_increment_coord(self, id, increment, speed):
+ """coord step mode
+
+ Args:
+ id: axis id 1 - 6.
+ increment: Coord increment value
+ speed: int (1 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=id, speed=speed)
+ value = self._coord2int(increment) if id <= 3 else self._angle2int(increment)
+ return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, id, [value], speed)
+
def set_HTS_gripper_torque(self, torque):
"""Set new adaptive gripper torque
@@ -674,6 +706,156 @@ def get_gpio_in(self, pin_no):
"""
return self._mesg(ProtocolCode.GET_GPIO_IN, pin_no, has_reply=True)
+ def angles_to_coords(self, angles):
+ """ Convert angles to coordinates
+
+ Args:
+ angles : A float list of all angle.
+
+ Return:
+ list: A float list of all coordinates.
+ """
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True)
+
+ def solve_inv_kinematics(self, target_coords, current_angles):
+ """ Convert target coordinates to angles
+
+ Args:
+ target_coords: A float list of all coordinates.
+ current_angles : A float list of all angle.
+
+ Return:
+ list: A float list of all angle.
+ """
+ angles = [self._angle2int(angle) for angle in current_angles]
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(target_coords[idx]))
+ for angle in target_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
+ def set_vision_mode(self, flag):
+ """Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode.
+ (Only for visual tracking function)
+
+ Args:
+ flag: 0/1; 0 - close; 1 - open
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag)
+ return self._mesg(ProtocolCode.SET_VISION_MODE, flag)
+
+ def is_torque_gripper(self):
+ """Whether it is a force-controlled gripper
+
+ Return:
+ 1 - Force control
+ 0 - Non-force control
+ """
+ return self.get_servo_data(7, 1)
+
+ def set_gripper_state(self, flag, speed, _type_1=None, is_torque=None):
+ """Set gripper switch state
+
+ Args:
+ flag (int): 0 - open, 1 - close, 254 - release
+ speed (int): 1 ~ 100
+ _type_1 (int): default 1
+ 1 : Adaptive gripper. default to adaptive gripper
+ 2 : 5 finger dexterous hand
+ 3 : Parallel gripper, this parameter can be omitted
+ 4 : Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag, speed=speed, _type_1=_type_1, is_torque=is_torque)
+ args = [flag, speed]
+ if _type_1 is not None:
+ args.append(_type_1)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_STATE, *args)
+
+ def set_gripper_value(self, gripper_value, speed, gripper_type=None, is_torque=None):
+ """Set gripper value
+
+ Args:
+ gripper_value (int): 0 ~ 100
+ speed (int): 1 ~ 100
+ gripper_type (int): default 1
+ 1: Adaptive gripper
+ 3: Parallel gripper, this parameter can be omitted
+ 4: Flexible gripper
+ is_torque (int): When there is no type parameter, this parameter can be omitted.
+ 1: Force control
+ 0: Non-force control
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_value=gripper_value, speed=speed,
+ gripper_type=gripper_type, is_torque=is_torque)
+ args = [gripper_value, speed]
+ if gripper_type is not None:
+ args.append(gripper_type)
+ if is_torque is not None:
+ args.append(is_torque)
+ return self._mesg(ProtocolCode.SET_GRIPPER_VALUE, *args, has_reply=True)
+
+ def drag_start_record(self): # TODO need test 2024/11/15
+ """Start track recording
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_START_RECORD, has_reply=True)
+
+ def drag_end_record(self): # TODO need test 2024/11/15
+ """End track recording
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_END_RECORD, has_reply=True)
+
+ def drag_get_record_data(self): # TODO need test 2024/11/15
+ """Get the recorded track
+
+ Return:
+ List of potential values (encoder values) and operating speeds of each joint
+ eg: [J1_encoder, J1_run_speed,J2_encoder, J2_run_speed,J3_encoder, J3_run_speed,J4_encoder, J4_run_speed,J5_
+ encoder, J5_run_speed,J6_encoder, J6_run_speed]
+ """
+
+ return self._mesg(ProtocolCode.DRAG_GET_RECORD_DATA, has_reply=True)
+
+ def drag_get_record_len(self): # TODO need test 2024/11/15
+ """Get the total number of recorded points
+
+ Return:
+ Recording queue length
+ """
+
+ return self._mesg(ProtocolCode.DRAG_GET_RECORD_LEN, has_reply=True)
+
+ def drag_clear_record_data(self): # TODO need test 2024/11/15
+ """Clear recording track
+
+ Return:
+ Recording queue length 0
+ """
+
+ return self._mesg(ProtocolCode.DRAG_CLEAR_RECORD_DATA, has_reply=True)
+
+ def get_reboot_count(self):
+ """Read reboot count
+
+ Return:
+ reboot count
+ """
+ return self._mesg(ProtocolCode.GET_REBOOT_COUNT, has_reply=True)
+
# Other
def wait(self, t):
time.sleep(t)
@@ -681,3 +863,6 @@ def wait(self, t):
def close(self):
self.sock.close()
+
+ def go_home(self):
+ return self.send_angles([0,0,0,0,0,0], 10)
diff --git a/pymycobot/mycobot320.py b/pymycobot/mycobot320.py
index 1bc8589..ef84e88 100644
--- a/pymycobot/mycobot320.py
+++ b/pymycobot/mycobot320.py
@@ -9,7 +9,7 @@
from pymycobot.log import setup_logging
from pymycobot.generate import CommandGenerator
from pymycobot.public import PublicCommandGenerator
-from pymycobot.common import ProtocolCode, write, read, ProGripper
+from pymycobot.common import ProtocolCode, write, read, ProGripper, MyHandGripper
from pymycobot.error import calibration_parameters
@@ -61,6 +61,7 @@ class MyCobot320(CommandGenerator):
get_servo_temps()
get_servo_last_pdi()
set_void_compensate()
+ get_robot_status()
# Coordinate transformation
set_tool_reference()
@@ -137,7 +138,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyCobot320, self)._mesg(genre, *args, **kwargs)
if self.thread_lock:
with self.lock:
@@ -146,18 +147,29 @@ def _mesg(self, genre, *args, **kwargs):
return self._res(real_command, has_reply, genre)
def _res(self, real_command, has_reply, genre):
- try_count = 0
- while try_count < 3:
+ if genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
self._write(self._flatten(real_command))
data = self._read(genre)
- if data is not None and data != b'':
- break
- try_count += 1
else:
- return -1
+ try_count = 0
+ while try_count < 3:
+ self._write(self._flatten(real_command))
+ data = self._read(genre, real_command=real_command)
+ if data is not None and data != b'':
+ break
+ try_count += 1
+ else:
+ return -1
if genre == ProtocolCode.SET_SSID_PWD:
- return None
+ return 1
res = self._process_received(data, genre)
+ if res is None:
+ return None
+ if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+ if res == [0]:
+ self._write(self._flatten(real_command))
+ data = self._read(genre, real_command=real_command)
+ res = self._process_received(data, genre)
if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [ProtocolCode.GET_BASIC_VERSION,
ProtocolCode.GET_JOINT_MIN_ANGLE,
ProtocolCode.GET_JOINT_MAX_ANGLE,
@@ -194,6 +206,10 @@ def _res(self, real_command, has_reply, genre):
]:
return self._process_single(res)
elif genre in [ProtocolCode.GET_TOQUE_GRIPPER]:
+ if res[-1] == 255 and res[-2] == 255:
+ self._write(self._flatten(real_command))
+ data = self._read(genre)
+ res = self._process_received(data, genre)
return self._process_high_low_bytes(res)
elif genre in [ProtocolCode.GET_ANGLES]:
return [self._int2angle(angle) for angle in res]
@@ -213,6 +229,8 @@ def _res(self, real_command, has_reply, genre):
return self._int2coord(res[0])
elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION, ProtocolCode.GET_ATOM_VERSION]:
return self._int2coord(self._process_single(res))
+ elif genre in [ProtocolCode.GET_REBOOT_COUNT]:
+ return self._process_high_low_bytes(res)
elif genre == ProtocolCode.GET_ANGLES_COORDS:
r = []
for index in range(len(res)):
@@ -286,7 +304,7 @@ def sync_send_angles(self, degrees, speed, timeout=15):
Args:
degrees: a list of degree values(List[float]), length 6.
speed: (int) 0 ~ 100
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_angles(degrees, speed)
@@ -304,7 +322,7 @@ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
coords: a list of coord values(List[float])
speed: (int) 0 ~ 100
mode: (int): 0 - angular（default）, 1 - linear
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_coords(coords, speed, mode)
@@ -330,6 +348,29 @@ def jog_rpy(self, end_direction, direction, speed):
self.calibration_parameters(class_name=self.__class__.__name__, end_direction=end_direction, speed=speed)
return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed)
+ def jog_increment_angle(self, joint_id, increment, speed):
+ """ angle step mode
+
+ Args:
+ joint_id: int 1-6.
+ increment: Angle increment value
+ speed: int (0 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, speed=speed)
+ return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
+
+ def jog_increment_coord(self, id, increment, speed):
+ """coord step mode
+
+ Args:
+ id: axis id 1 - 6.
+ increment: Coord increment value
+ speed: int (1 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=id, speed=speed)
+ value = self._coord2int(increment) if id <= 3 else self._angle2int(increment)
+ return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, id, [value], speed)
+
# Basic for raspberry pi.
def gpio_init(self):
"""Init GPIO module, and set BCM mode."""
@@ -371,18 +412,18 @@ def set_joint_min(self, id, angle):
return self._mesg(ProtocolCode.SET_JOINT_MIN, id, angle)
# Atom Gripper IO
- def init_eletric_gripper(self): # TODO 22-5-19 need test
+ def init_electric_gripper(self): # TODO 22-5-19 need test
"""Electric gripper initialization (it needs to be initialized once after inserting and removing the gripper"""
- return self._mesg(ProtocolCode.INIT_ELETRIC_GRIPPER)
+ return self._mesg(ProtocolCode.INIT_ELECTRIC_GRIPPER)
- def set_eletric_gripper(self, status): # TODO 22-5-19 need test
+ def set_electric_gripper(self, status): # TODO 22-5-19 need test
"""Set Electric Gripper Mode
Args:
status: 0 - open, 1 - close.
"""
self.calibration_parameters(class_name=self.__class__.__name__, status=status)
- return self._mesg(ProtocolCode.SET_ELETRIC_GRIPPER, status)
+ return self._mesg(ProtocolCode.SET_ELECTRIC_GRIPPER, status)
def set_gripper_mode(self, mode):
"""Set gripper mode
@@ -674,8 +715,8 @@ def set_pro_gripper_speed(self, gripper_id, speed):
self.calibration_parameters(class_name=self.__class__.__name__, gripper_speed=[gripper_id, speed])
return self.set_pro_gripper(gripper_id, ProGripper.SET_GRIPPER_SPEED, speed)
- def get_pro_gripper_default_speed(self, gripper_id):
- """ Get the default gripper speed
+ def get_pro_gripper_speed(self, gripper_id):
+ """ Get the gripper speed
Args:
gripper_id (int): 1 ~ 254
@@ -684,10 +725,10 @@ def get_pro_gripper_default_speed(self, gripper_id):
speed (int): 1 ~ 100
"""
self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
- return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_DEFAULT_SPEED)
+ return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_SPEED)
def set_pro_gripper_abs_angle(self, gripper_id, gripper_angle):
- """ Set the gripper speed
+ """ Set the gripper absolute angle
Args:
gripper_id (int): 1 ~ 254
@@ -731,6 +772,506 @@ def get_atom_version(self):
"""
return self._mesg(ProtocolCode.GET_ATOM_VERSION, has_reply=True)
+ def get_robot_status(self):
+ """Get robot status
+ """
+ return self._mesg(ProtocolCode.GET_ROBOT_STATUS, has_reply=True)
+
+ def set_vision_mode(self, flag):
+ """Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode.
+ (Only for visual tracking function)
+
+ Args:
+ flag: 0/1; 0 - close; 1 - open
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag)
+ return self._mesg(ProtocolCode.SET_VISION_MODE, flag)
+
+ # myHand Gripper Control
+ def get_hand_firmware_major_version(self, gripper_id):
+ """Read the firmware major version number
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_MAJOR_FIRMWARE_VERSION])
+
+ def get_hand_firmware_minor_version(self, gripper_id):
+ """Read the firmware minor version number
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_MINOR_FIRMWARE_VERSION])
+
+ def set_hand_gripper_id(self, gripper_id, id_value):
+ """Set the gripper ID
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ id_value (int): 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id, set_id=id_value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_ID], [id_value])
+
+ def get_hand_gripper_id(self, gripper_id):
+ """Get the gripper ID
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+
+ Return:
+ gripper ID (int): 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_ID])
+
+ def set_hand_gripper_angle(self, gripper_id, joint_id, gripper_angle):
+ """Set the angle of the single joint of the gripper
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ joint_id (int): 1 ~ 6
+ gripper_angle (int): 0 ~ 100
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_angle=[gripper_id, gripper_angle],
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_ANGLE],
+ [joint_id], [gripper_angle])
+
+ def get_hand_gripper_angle(self, gripper_id, joint_id):
+ """Get the angle of the single joint of the gripper
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ gripper_angle (int): 0 ~ 100
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_ANGLE],
+ [joint_id])
+
+ def set_hand_gripper_angles(self, gripper_id, gripper_angles, speed):
+ """Set the angle of the single joint of the gripper
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ gripper_angles (list): A list of integers, length 6, each value range 0 ~ 100
+ speed (int): 0 ~ 100
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_speed=[gripper_id, speed],
+ gripper_angles=gripper_angles)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_ANGLES],
+ [gripper_angles], [speed])
+
+ def get_hand_gripper_angles(self, gripper_id):
+ """Set the angle of the single joint of the gripper
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+
+ Return:
+ gripper_angles (list): A list of integers, length 6
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_ALL_ANGLES])
+
+ def set_hand_gripper_torque(self, gripper_id, joint_id, torque_value):
+ """ Setting gripper torque
+
+ Args:
+ joint_id (int): 1 ~ 6
+ gripper_id (int): 1 ~ 254
+ torque_value (int): 100 ~ 300
+
+ Return:
+ 0: Set failed
+ 1: Set successful
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, torque_value=[gripper_id, torque_value])
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_TORQUE],
+ [joint_id], [torque_value])
+
+ def get_hand_gripper_torque(self, gripper_id, joint_id):
+ """ Setting gripper torque
+
+ Args:
+ joint_id (int): 1 ~ 6
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ torque_value (int): 100 ~ 300
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_TORQUE],
+ [joint_id])
+
+ def set_hand_gripper_calibrate(self, gripper_id, joint_id):
+ """ Setting the gripper jaw zero position
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_CALIBRATION],
+ [joint_id], [0])
+
+ def get_hand_gripper_status(self, gripper_id):
+ """ Get the clamping status of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ 0 - Moving
+ 1 - Stopped moving, no clamping detected
+ 2 - Stopped moving, clamping detected
+ 3 - After clamping detected, the object fell
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_STATUS])
+
+ def set_hand_gripper_enabled(self, gripper_id, flag):
+ """ Set the enable state of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ flag (int): 1 ~ 6
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id, flag=flag)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_ENABLED], [flag])
+
+ def set_hand_gripper_speed(self, gripper_id, joint_id, speed):
+ """ Set the speed of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ speed (int): 1 ~ 100
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_speed=[gripper_id, speed],
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_SPEED],
+ [joint_id], [speed])
+
+ def get_hand_gripper_default_speed(self, gripper_id, joint_id):
+ """ Get the default speed of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ default speed (int): 1 ~ 100
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_DEFAULT_SPEED],
+ [joint_id])
+
+ def set_hand_gripper_pinch_action(self, gripper_id, pinch_mode):
+ """ Set the pinching action of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ pinch_mode (int):
+ 0 - Index finger and thumb pinch
+ 1 - Middle finger and thumb pinch
+ 2 - Three-finger grip
+ 3 - Two-finger grip
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id, pinch_mode=pinch_mode)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_PINCH_ACTION],
+ [pinch_mode])
+
+ def set_hand_gripper_p(self, gripper_id, joint_id, value):
+ """ Set the P value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, gripper_p=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_P],
+ [joint_id], [value])
+
+ def get_hand_gripper_p(self, gripper_id, joint_id):
+ """ Get the P value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ P value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_P],
+ [joint_id])
+
+ def set_hand_gripper_d(self, gripper_id, joint_id, value):
+ """ Set the D value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, gripper_d=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_D],
+ [joint_id], [value])
+
+ def get_hand_gripper_d(self, gripper_id, joint_id):
+ """ Get the D value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ D value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_D],
+ [joint_id])
+
+ def set_hand_gripper_i(self, gripper_id, joint_id, value):
+ """ Set the I value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, gripper_i=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_I],
+ [joint_id], [value])
+
+ def get_hand_gripper_i(self, gripper_id, joint_id):
+ """ Get the I value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ I value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_I],
+ [joint_id])
+
+ def set_hand_gripper_min_pressure(self, gripper_id, joint_id, value):
+ """ Set the minimum starting force of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, min_pressure=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_MIN_PRESSURE],
+ [joint_id], [value])
+
+ def get_hand_gripper_min_pressure(self, gripper_id, joint_id):
+ """ Set the minimum starting force of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ min pressure value (int): 0 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_MIN_PRESSURE],
+ [joint_id])
+
+ def set_hand_gripper_clockwise(self, gripper_id, joint_id, value):
+ """ Set the clockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 16
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, clockwise=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_CLOCKWISE],
+ [joint_id], [value])
+
+ def get_hand_gripper_clockwise(self, gripper_id, joint_id):
+ """ Get the clockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ value (int): 0 ~ 16
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_CLOCKWISE],
+ [joint_id])
+
+ def set_hand_gripper_counterclockwise(self, gripper_id, joint_id, value):
+ """ Set the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 16
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, clockwise=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_COUNTERCLOCKWISE],
+ [joint_id], [value])
+
+ def get_hand_gripper_counterclockwise(self, gripper_id, joint_id):
+ """ Get the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ value (int): 0 ~ 16
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_COUNTERCLOCKWISE],
+ [joint_id])
+
+ def get_hand_single_pressure_sensor(self, gripper_id, finger_id):
+ """ Get the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ finger_id (int): 1 ~ 5
+
+ Return:
+ int: 0 ~ 4096
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_finger_id=finger_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_SINGLE_PRESSURE_SENSOR],
+ [finger_id])
+
+ def get_hand_all_pressure_sensor(self, gripper_id):
+ """ Get the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ int: 0 ~ 4096
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_SINGLE_PRESSURE_SENSOR])
+
+ def get_reboot_count(self):
+ """Read reboot count
+
+ Return:
+ reboot count
+ """
+ return self._mesg(ProtocolCode.GET_REBOOT_COUNT, has_reply=True)
+
+ def set_hand_gripper_pinch_action_speed_consort(self, gripper_id, pinch_pose, rank_mode, idle_flag=None):
+ """ Setting the gripper pinching action-speed coordination
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ pinch_pose (int): 0 ~ 4
+ 0: All joints return to zero
+ 1: Index finger and thumb pinch together
+ 2: Middle finger and thumb pinch together
+ 3: Index finger and middle finger pinch together
+ 4: Three fingers together
+ rank_mode (int): 0 ~ 5
+ The degree of closure,the higher the level, the more closed
+ idle_flag (int): default None, 0 ~ 4
+ Idle flag. By default, there is no such byte. When this byte is 1, the idle finger can be freely manipulated.
+
+ """
+ if idle_flag is None:
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ pinch_pose=pinch_pose, rank_mode=rank_mode)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id,
+ [MyHandGripper.SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT], pinch_pose, rank_mode)
+ else:
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ pinch_pose=pinch_pose, rank_mode=rank_mode, idle_flag=idle_flag)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id,
+ [MyHandGripper.SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT], pinch_pose, rank_mode, idle_flag)
+
+ def angles_to_coords(self, angles):
+ """ Convert angles to coordinates
+
+ Args:
+ angles : A float list of all angle.
+
+ Return:
+ list: A float list of all coordinates.
+ """
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True)
+
+ def solve_inv_kinematics(self, target_coords, current_angles):
+ """ Convert target coordinates to angles
+
+ Args:
+ target_coords: A float list of all coordinates.
+ current_angles : A float list of all angle.
+
+ Return:
+ list: A float list of all angle.
+ """
+ angles = [self._angle2int(angle) for angle in current_angles]
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(target_coords[idx]))
+ for angle in target_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
# Other
def wait(self, t):
time.sleep(t)
@@ -741,3 +1282,7 @@ def close(self):
def open(self):
self._serial_port.open()
+
+ def go_home(self):
+ return self.send_angles([0,0,0,0,0,0], 10)
+
diff --git a/pymycobot/mycobot320socket.py b/pymycobot/mycobot320socket.py
index 0e8b10d..9f94660 100644
--- a/pymycobot/mycobot320socket.py
+++ b/pymycobot/mycobot320socket.py
@@ -7,7 +7,7 @@
import threading
from pymycobot.generate import CommandGenerator
-from pymycobot.common import ProtocolCode, write, read, ProGripper
+from pymycobot.common import ProtocolCode, write, read, ProGripper, MyHandGripper
from pymycobot.error import calibration_parameters
@@ -43,8 +43,8 @@ class MyCobot320Socket(CommandGenerator):
Look at parent class: `CommandGenerator`.
# Atom IO
- init_eletric_gripper()
- set_eletric_gripper()
+ init_electric_gripper()
+ set_electric_gripper()
set_gripper_mode()
get_gripper_mode()
Other at parent class: `CommandGenerator`.
@@ -60,6 +60,7 @@ class MyCobot320Socket(CommandGenerator):
get_servo_temps()
get_servo_last_pdi()
set_void_compensate()
+ get_robot_status()
# Coordinate transformation
set_tool_reference()
@@ -130,28 +131,37 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyCobot320Socket, self)._mesg(genre, *args, **kwargs)
# [254,...,255]
with self.lock:
# data = self._write(self._flatten(real_command), "socket")
# # if has_reply:
# data = self._read(genre, method='socket')
- try_count = 0
- while try_count < 3:
+ if genre == ProtocolCode.SET_SSID_PWD or genre == ProtocolCode.GET_SSID_PWD:
self._write(self._flatten(real_command), "socket")
data = self._read(genre, method='socket')
- if data is not None and data != b'':
- break
- try_count += 1
else:
- return -1
+ try_count = 0
+ while try_count < 3:
+ self._write(self._flatten(real_command), "socket")
+ data = self._read(genre, method='socket', real_command=real_command)
+ if data is not None and data != b'':
+ break
+ try_count += 1
+ else:
+ return -1
if genre == ProtocolCode.SET_SSID_PWD:
- return None
+ return 1
res = self._process_received(data, genre)
- if res == []:
+ if res is None:
return None
- elif res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
+ if genre == ProtocolCode.SET_TOQUE_GRIPPER:
+ if res == [0]:
+ self._write(self._flatten(real_command))
+ data = self._read(genre, real_command=real_command)
+ res = self._process_received(data, genre)
+ if res is not None and isinstance(res, list) and len(res) == 1 and genre not in [
ProtocolCode.GET_BASIC_VERSION,
ProtocolCode.GET_JOINT_MIN_ANGLE,
ProtocolCode.GET_JOINT_MAX_ANGLE,
@@ -213,6 +223,8 @@ def _mesg(self, genre, *args, **kwargs):
elif genre in [ProtocolCode.GET_BASIC_VERSION, ProtocolCode.SOFTWARE_VERSION,
ProtocolCode.GET_ATOM_VERSION]:
return self._int2coord(self._process_single(res))
+ elif genre in [ProtocolCode.GET_REBOOT_COUNT]:
+ return self._process_high_low_bytes(res)
elif genre == ProtocolCode.GET_ANGLES_COORDS:
r = []
for index in range(len(res)):
@@ -286,7 +298,7 @@ def sync_send_angles(self, degrees, speed, timeout=15):
Args:
degrees: a list of degree values(List[float]), length 6.
speed: (int) 0 ~ 100
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_angles(degrees, speed)
@@ -304,7 +316,7 @@ def sync_send_coords(self, coords, speed, mode=0, timeout=15):
coords: a list of coord values(List[float])
speed: (int) 0 ~ 100
mode: (int): 0 - angular（default）, 1 - linear
- timeout: default 7s.
+ timeout: default 15s.
"""
t = time.time()
self.send_coords(coords, speed, mode)
@@ -330,6 +342,29 @@ def jog_rpy(self, end_direction, direction, speed):
self.calibration_parameters(class_name=self.__class__.__name__, end_direction=end_direction, speed=speed)
return self._mesg(ProtocolCode.JOG_ABSOLUTE, end_direction, direction, speed)
+ def jog_increment_angle(self, joint_id, increment, speed):
+ """ angle step mode
+
+ Args:
+ joint_id: int 1-6.
+ increment: Angle increment value
+ speed: int (0 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=joint_id, speed=speed)
+ return self._mesg(ProtocolCode.JOG_INCREMENT, joint_id, [self._angle2int(increment)], speed)
+
+ def jog_increment_coord(self, id, increment, speed):
+ """coord step mode
+
+ Args:
+ id: axis id 1 - 6.
+ increment: Coord increment value
+ speed: int (1 - 100)
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, id=id, speed=speed)
+ value = self._coord2int(increment) if id <= 3 else self._angle2int(increment)
+ return self._mesg(ProtocolCode.JOG_INCREMENT_COORD, id, [value], speed)
+
# Basic for raspberry pi.
def gpio_init(self):
"""Init GPIO module, and set BCM mode."""
@@ -372,18 +407,18 @@ def set_joint_min(self, id, angle):
return self._mesg(ProtocolCode.SET_JOINT_MIN, id, angle)
# Atom Gripper IO
- def init_eletric_gripper(self): # TODO 22-5-19 need test
+ def init_electric_gripper(self): # TODO 22-5-19 need test
"""Electric gripper initialization (it needs to be initialized once after inserting and removing the gripper"""
- return self._mesg(ProtocolCode.INIT_ELETRIC_GRIPPER)
+ return self._mesg(ProtocolCode.INIT_ELECTRIC_GRIPPER)
- def set_eletric_gripper(self, status): # TODO 22-5-19 need test
+ def set_electric_gripper(self, status): # TODO 22-5-19 need test
"""Set Electric Gripper Mode
Args:
status: 0 - open, 1 - close.
"""
self.calibration_parameters(class_name=self.__class__.__name__, status=status)
- return self._mesg(ProtocolCode.SET_ELETRIC_GRIPPER, status)
+ return self._mesg(ProtocolCode.SET_ELECTRIC_GRIPPER, status)
def set_gripper_mode(self, mode):
"""Set gripper mode
@@ -723,7 +758,7 @@ def get_pro_gripper_default_speed(self, gripper_id):
return self.get_pro_gripper(gripper_id, ProGripper.GET_GRIPPER_DEFAULT_SPEED)
def set_pro_gripper_abs_angle(self, gripper_id, gripper_angle):
- """ Set the gripper speed
+ """ Set the gripper absolute angle
Args:
gripper_id (int): 1 ~ 254
@@ -767,6 +802,506 @@ def get_atom_version(self):
"""
return self._mesg(ProtocolCode.GET_ATOM_VERSION, has_reply=True)
+ def get_robot_status(self):
+ """Get robot status
+ """
+ return self._mesg(ProtocolCode.GET_ROBOT_STATUS, has_reply=True)
+
+ def set_vision_mode(self, flag):
+ """Set the visual tracking mode to limit the posture flipping of send_coords in refresh mode.
+ (Only for visual tracking function)
+
+ Args:
+ flag: 0/1; 0 - close; 1 - open
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, flag=flag)
+ return self._mesg(ProtocolCode.SET_VISION_MODE, flag)
+
+ # myHand Gripper Control
+ def get_hand_firmware_major_version(self, gripper_id):
+ """Read the firmware major version number
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_MAJOR_FIRMWARE_VERSION])
+
+ def get_hand_firmware_minor_version(self, gripper_id):
+ """Read the firmware minor version number
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_MINOR_FIRMWARE_VERSION])
+
+ def set_hand_gripper_id(self, gripper_id, id_value):
+ """Set the gripper ID
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ id_value (int): 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id, set_id=id_value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_ID], [id_value])
+
+ def get_hand_gripper_id(self, gripper_id):
+ """Get the gripper ID
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+
+ Return:
+ gripper ID (int): 1 ~ 254
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_ID])
+
+ def set_hand_gripper_angle(self, gripper_id, joint_id, gripper_angle):
+ """Set the angle of the single joint of the gripper
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ joint_id (int): 1 ~ 6
+ gripper_angle (int): 0 ~ 100
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_angle=[gripper_id, gripper_angle],
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_ANGLE],
+ [joint_id], [gripper_angle])
+
+ def get_hand_gripper_angle(self, gripper_id, joint_id):
+ """Get the angle of the single joint of the gripper
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ gripper_angle (int): 0 ~ 100
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_ANGLE],
+ [joint_id])
+
+ def set_hand_gripper_angles(self, gripper_id, gripper_angles, speed):
+ """Set the angle of the single joint of the gripper
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+ gripper_angles (list): A list of integers, length 6, each value range 0 ~ 100
+ speed (int): 0 ~ 100
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_speed=[gripper_id, speed],
+ gripper_angles=gripper_angles)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_ANGLES],
+ [gripper_angles], [speed])
+
+ def get_hand_gripper_angles(self, gripper_id):
+ """Set the angle of the single joint of the gripper
+
+ Args:
+ gripper_id (int) : 1 ~ 254
+
+ Return:
+ gripper_angles (list): A list of integers, length 6
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_ALL_ANGLES])
+
+ def set_hand_gripper_torque(self, gripper_id, joint_id, torque_value):
+ """ Setting gripper torque
+
+ Args:
+ joint_id (int): 1 ~ 6
+ gripper_id (int): 1 ~ 254
+ torque_value (int): 100 ~ 300
+
+ Return:
+ 0: Set failed
+ 1: Set successful
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, torque_value=[gripper_id, torque_value])
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_TORQUE],
+ [joint_id], [torque_value])
+
+ def get_hand_gripper_torque(self, gripper_id, joint_id):
+ """ Setting gripper torque
+
+ Args:
+ joint_id (int): 1 ~ 6
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ torque_value (int): 100 ~ 300
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_TORQUE],
+ [joint_id])
+
+ def set_hand_gripper_calibrate(self, gripper_id, joint_id):
+ """ Setting the gripper jaw zero position
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_CALIBRATION],
+ [joint_id], [0])
+
+ def get_hand_gripper_status(self, gripper_id):
+ """ Get the clamping status of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ 0 - Moving
+ 1 - Stopped moving, no clamping detected
+ 2 - Stopped moving, clamping detected
+ 3 - After clamping detected, the object fell
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_STATUS])
+
+ def set_hand_gripper_enabled(self, gripper_id, flag):
+ """ Set the enable state of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ flag (int): 1 ~ 6
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id, flag=flag)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_ENABLED], [flag])
+
+ def set_hand_gripper_speed(self, gripper_id, joint_id, speed):
+ """ Set the speed of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ speed (int): 1 ~ 100
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_speed=[gripper_id, speed],
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_SPEED],
+ [joint_id], [speed])
+
+ def get_hand_gripper_default_speed(self, gripper_id, joint_id):
+ """ Get the default speed of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ default speed (int): 1 ~ 100
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_DEFAULT_SPEED],
+ [joint_id])
+
+ def set_hand_gripper_pinch_action(self, gripper_id, pinch_mode):
+ """ Set the pinching action of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ pinch_mode (int):
+ 0 - Index finger and thumb pinch
+ 1 - Middle finger and thumb pinch
+ 2 - Three-finger grip
+ 3 - Two-finger grip
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id, pinch_mode=pinch_mode)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_PINCH_ACTION],
+ [pinch_mode])
+
+ def set_hand_gripper_p(self, gripper_id, joint_id, value):
+ """ Set the P value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, gripper_p=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_P],
+ [joint_id], [value])
+
+ def get_hand_gripper_p(self, gripper_id, joint_id):
+ """ Get the P value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ P value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_P],
+ [joint_id])
+
+ def set_hand_gripper_d(self, gripper_id, joint_id, value):
+ """ Set the D value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, gripper_d=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_D],
+ [joint_id], [value])
+
+ def get_hand_gripper_d(self, gripper_id, joint_id):
+ """ Get the D value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ D value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_D],
+ [joint_id])
+
+ def set_hand_gripper_i(self, gripper_id, joint_id, value):
+ """ Set the I value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, gripper_i=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_I],
+ [joint_id], [value])
+
+ def get_hand_gripper_i(self, gripper_id, joint_id):
+ """ Get the I value of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ I value (int): 0 ~ 150
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_I],
+ [joint_id])
+
+ def set_hand_gripper_min_pressure(self, gripper_id, joint_id, value):
+ """ Set the minimum starting force of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, min_pressure=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_MIN_PRESSURE],
+ [joint_id], [value])
+
+ def get_hand_gripper_min_pressure(self, gripper_id, joint_id):
+ """ Set the minimum starting force of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ min pressure value (int): 0 ~ 254
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_MIN_PRESSURE],
+ [joint_id])
+
+ def set_hand_gripper_clockwise(self, gripper_id, joint_id, value):
+ """ Set the clockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 16
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, clockwise=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_CLOCKWISE],
+ [joint_id], [value])
+
+ def get_hand_gripper_clockwise(self, gripper_id, joint_id):
+ """ Get the clockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ value (int): 0 ~ 16
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_CLOCKWISE],
+ [joint_id])
+
+ def set_hand_gripper_counterclockwise(self, gripper_id, joint_id, value):
+ """ Set the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+ value (int): 0 ~ 16
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id, clockwise=value)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.SET_HAND_GRIPPER_COUNTERCLOCKWISE],
+ [joint_id], [value])
+
+ def get_hand_gripper_counterclockwise(self, gripper_id, joint_id):
+ """ Get the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ joint_id (int): 1 ~ 6
+
+ Return:
+ value (int): 0 ~ 16
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_joint_id=joint_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_GRIPPER_COUNTERCLOCKWISE],
+ [joint_id])
+
+ def get_hand_single_pressure_sensor(self, gripper_id, finger_id):
+ """ Get the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ finger_id (int): 1 ~ 5
+
+ Return:
+ int: 0 ~ 4096
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ gripper_finger_id=finger_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_SINGLE_PRESSURE_SENSOR],
+ [finger_id])
+
+ def get_hand_all_pressure_sensor(self, gripper_id):
+ """ Get the counterclockwise runnable error of the single joint of the gripper
+
+ Args:
+ gripper_id (int): 1 ~ 254
+
+ Return:
+ int: 0 ~ 4096
+
+ """
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id)
+ return self._mesg(ProtocolCode.GET_TOQUE_GRIPPER, gripper_id, [MyHandGripper.GET_HAND_SINGLE_PRESSURE_SENSOR])
+
+ def get_reboot_count(self):
+ """Read reboot count
+
+ Return:
+ reboot count
+ """
+ return self._mesg(ProtocolCode.GET_REBOOT_COUNT, has_reply=True)
+
+ def set_hand_gripper_pinch_action_speed_consort(self, gripper_id, pinch_pose, rank_mode, idle_flag=None):
+ """ Setting the gripper pinching action-speed coordination
+
+ Args:
+ gripper_id (int): 1 ~ 254
+ pinch_pose (int): 0 ~ 4
+ 0: All joints return to zero
+ 1: Index finger and thumb pinch together
+ 2: Middle finger and thumb pinch together
+ 3: Index finger and middle finger pinch together
+ 4: Three fingers together
+ rank_mode (int): 0 ~ 5
+ The degree of closure,the higher the level, the more closed
+ idle_flag (int): default None, 0 ~ 4
+ Idle flag. By default, there is no such byte. When this byte is 1, the idle finger can be freely manipulated.
+
+ """
+ if idle_flag is None:
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ pinch_pose=pinch_pose, rank_mode=rank_mode)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id,
+ [MyHandGripper.SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT], pinch_pose, rank_mode)
+ else:
+ self.calibration_parameters(class_name=self.__class__.__name__, gripper_id=gripper_id,
+ pinch_pose=pinch_pose, rank_mode=rank_mode, idle_flag=idle_flag)
+ return self._mesg(ProtocolCode.SET_TOQUE_GRIPPER, gripper_id,
+ [MyHandGripper.SET_HAND_GRIPPER_PINCH_ACTION_SPEED_CONSORT], pinch_pose, rank_mode, idle_flag)
+
+ def angles_to_coords(self, angles):
+ """ Convert angles to coordinates
+
+ Args:
+ angles : A float list of all angle.
+
+ Return:
+ list: A float list of all coordinates.
+ """
+ angles = [self._angle2int(angle) for angle in angles]
+ return self._mesg(ProtocolCode.GET_COORDS, angles, has_reply=True)
+
+ def solve_inv_kinematics(self, target_coords, current_angles):
+ """ Convert target coordinates to angles
+
+ Args:
+ target_coords: A float list of all coordinates.
+ current_angles : A float list of all angle.
+
+ Return:
+ list: A float list of all angle.
+ """
+ angles = [self._angle2int(angle) for angle in current_angles]
+ coord_list = []
+ for idx in range(3):
+ coord_list.append(self._coord2int(target_coords[idx]))
+ for angle in target_coords[3:]:
+ coord_list.append(self._angle2int(angle))
+ return self._mesg(ProtocolCode.SOLVE_INV_KINEMATICS, coord_list, angles, has_reply=True)
+
# Other
def wait(self, t):
time.sleep(t)
@@ -774,3 +1309,6 @@ def wait(self, t):
def close(self):
self.sock.close()
+
+ def go_home(self):
+ return self.send_angles([0,0,0,0,0,0], 10)
diff --git a/pymycobot/mycobotpro630.py b/pymycobot/mycobotpro630.py
index 858d906..adfc92c 100644
--- a/pymycobot/mycobotpro630.py
+++ b/pymycobot/mycobotpro630.py
@@ -10,6 +10,7 @@
import logging
import os
import sys
+
import crc
from pymycobot.log import setup_logging
@@ -61,6 +62,7 @@ def is_debian_os():
import linuxcnc as elerob
import hal
import can
+ from can import Message
class JogMode(Enum):
JOG_JOINT = elerob.ANGULAR - 1
@@ -591,18 +593,38 @@ def _send_can(self, data, can_id=0x007, timeout=0.5):
except can.CanError:
print("Error: Cannot send can message")
- def _receive_can(self, msg_data=None, timeout=0.5, num_tries=1000):
+ def _receive_can(
+ self,
+ msg_data=None,
+ timeout=0.5,
+ num_tries=1000,
+ destroy_can=True,
+ messages_num=1,
+ ):
"""Receives next message from CAN bus.
Args:
- timeout (float, optional): How long time to receive message. Defaults to 0.5.
+ msg_data (None | List[int], optional): message to look for.
+ Defaults to None (look for any message). If multiple messages
+ are received, only the first one is compared to.
+ timeout (float, optional): How long time to receive message.
+ Defaults to 0.5.
+ num_tries (int, optional): how many times to try to receive
+ specified message. Defaults to 1000.
+ destroy_can (bool, optional): if need to destroy CAN BUS instance
+ after receiving message. Defaults to True.
+ messages_num (int, optional): how many messages to read and return.
+ If number of messages is more than 1, list of messages is
+ returned. Defaults to 1.
Returns:
- Message | None: CAN message or None (if no message could be received).
+ Message | List[Message] | None: CAN message or None (if no message could be received).
"""
msg_data = msg_data or []
msg = None
+ msg_list = []
self._init_can()
+
for _ in range(num_tries):
msg = self.bus.recv(timeout)
msg_found = True
@@ -615,10 +637,23 @@ def _receive_can(self, msg_data=None, timeout=0.5, num_tries=1000):
msg_found = False
if msg_found:
break
- self._destroy_can()
+
+ if messages_num > 1 and msg_found:
+ msg_list.append(msg)
+ for i in range(messages_num - 1):
+ msg = self.bus.recv(timeout)
+ msg_list.append(msg)
+
+ if destroy_can:
+ self._destroy_can()
+
if not msg_found:
msg = None
- return msg
+
+ if messages_num == 1:
+ return msg
+ else:
+ return msg_list
def _init_robot(self):
"""Initializes robot parameters."""
@@ -2781,7 +2816,27 @@ def tool_set_gripper_mode(self, mode):
"""
self._send_can([0x02, 0x6D, mode])
+ def tool_set_gripper_electric_init(self):
+ """Inits Electric Gripper. Need to call it before other electric gripper operation."""
+ self._send_can([0x01, 0x6C])
+
+ def tool_set_gripper_electric_open(self):
+ """Opens Electric Gripper."""
+ self._send_can([0x02, 0x6A, 0x00])
+
+ def tool_set_gripper_electric_close(self):
+ """Closes Electric Gripper."""
+ self._send_can([0x02, 0x6A, 0x01])
+
def tool_gripper_pro_set_angle(self, angle):
+ """Sets current angle of Pro Gripper.
+
+ Args:
+ angle (int): angle to set
+
+ Returns:
+ bool: True if success, False otherwise
+ """
if angle < 0 or angle > 100:
return False
command = [254, 254, 8, 14, 6, 0, 11, 0, angle]
@@ -2789,23 +2844,52 @@ def tool_gripper_pro_set_angle(self, angle):
command.extend([(crc16_value >> 8), (crc16_value & 0xFF)])
self.tool_serial_write_data(command)
ret = self.tool_serial_read_data(11)
- return bool(ret[8])
+ if len(ret) == 11:
+ return bool(ret[8])
+
+ return False
def tool_gripper_pro_get_angle(self):
+ """Returns current angle of Pro Gripper.
+
+ Returns:
+ int: current angle
+ """
command = [254, 254, 8, 14, 3, 0, 12, 0, 0]
crc16_value = crc.Calculator(crc.Crc16.MODBUS.value).checksum(bytes(command))
command.extend([(crc16_value >> 8), (crc16_value & 0xFF)])
self.tool_serial_write_data(command)
ret = self.tool_serial_read_data(11)
- return int((ret[7] << 8) | (ret[8]))
+ if len(ret) == 11:
+ return int((ret[7] << 8) | (ret[8]))
+
+ return -1
def tool_gripper_pro_open(self):
+ """Fully opens Pro Gripper.
+
+ Returns:
+ bool: True if success, False otherwise.
+ """
return self.tool_gripper_pro_set_angle(100)
def tool_gripper_pro_close(self):
+ """Fully closes Pro Gripper.
+
+ Returns:
+ bool: True if success, False otherwise.
+ """
return self.tool_gripper_pro_set_angle(0)
def tool_gripper_pro_set_torque(self, torque_value):
+ """Sets torque of Pro Gripper.
+
+ Args:
+ torque_value (int): torque value between 100 and 300
+
+ Returns:
+ bool: True if success, False otherwise
+ """
if torque_value < 100 or torque_value > 300:
return False
command = [
@@ -2826,6 +2910,11 @@ def tool_gripper_pro_set_torque(self, torque_value):
return bool(ret[8])
def tool_gripper_pro_get_torque(self):
+ """Returns current torque value of Pro Gripper.
+
+ Returns:
+ int: current torque value
+ """
command = [254, 254, 8, 14, 3, 0, 28, 0, 0]
crc16_value = crc.Calculator(crc.Crc16.MODBUS.value).checksum(bytes(command))
command.extend([(crc16_value >> 8), (crc16_value & 0xFF)])
@@ -2833,6 +2922,89 @@ def tool_gripper_pro_get_torque(self):
ret = self.tool_serial_read_data(11)
return int((ret[7] << 8) | (ret[8]))
+ def tool_hand_set_angles(self, angles, speed):
+ return self.tool_set_value(
+ 14,
+ 45,
+ [
+ 0,
+ angles[0],
+ 0,
+ angles[1],
+ 0,
+ angles[2],
+ 0,
+ angles[3],
+ 0,
+ angles[4],
+ 0,
+ angles[5],
+ 0,
+ speed,
+ ],
+ )
+
+ def tool_hand_open(self, speed=10):
+ return self.tool_hand_set_angles([0, 0, 0, 0, 0, 0], speed)
+
+ def tool_hand_close(self, speed=10):
+ return self.tool_hand_set_angles([100, 100, 100, 100, 100, 100], speed)
+
+ def tool_set_value(self, id, command, parameter):
+ """_summary_
+
+ send_angles[0,0,0,0,0,0] sp 10:
+ 07 0xD8 04 14 00 45 00 00
+ 07 0xD8 03 00 00 00 00 00
+ 07 0xD8 02 00 00 00 00 00
+ 04 0xD8 01 00 0a
+
+ Args:
+ id (_type_): _description_
+ command (_type_): _description_
+ parameter (_type_): _description_
+ """
+ command_bytes = command.to_bytes(2, "big")
+ if parameter is int:
+ parameter_bytes = parameter.to_bytes(2, "big")
+ else:
+ parameter_bytes = [id, command_bytes[0], command_bytes[1]] + parameter
+ messages_number = math.ceil(len(parameter_bytes) / 5)
+ for i, sequence in enumerate(range(messages_number, 0, -1)):
+ msg_len = 2 + 5
+ if sequence == 1:
+ msg_len = 2 + (len(parameter_bytes) % 5)
+ can_msg = [msg_len, 0xD8, sequence]
+ can_msg.extend(parameter_bytes[i * 5 : (i + 1) * 5])
+ self._send_can(can_msg)
+
+ msg = self._receive_can([0x02, 0xD8])
+ if msg is Message:
+ return bool(msg.data[2])
+
+ return False
+
+ def tool_get_value(self, id, command, parameter):
+ command_bytes = command.to_bytes(2, "big")
+ parameter_bytes = parameter.to_bytes(2, "big")
+ self._send_can(
+ [
+ 0x06,
+ 0xD9,
+ id,
+ command_bytes[0],
+ command_bytes[1],
+ parameter_bytes[0],
+ parameter_bytes[1],
+ ]
+ )
+ msg = self._receive_can([0x06, 0xD9, id, command_bytes[0], command_bytes[1]])
+ if msg is Message:
+ ret_value = int.from_bytes([msg.data[5], msg.data[6]], "big")
+ return ret_value
+
+ return -1
+
def tool_serial_restore(self):
"""Restore ESP32 serial."""
self._send_can([0x01, 0xB1])
@@ -2870,11 +3042,18 @@ def tool_serial_read_data(self, n):
"""
self._send_can([0x02, 0xB4, n])
data = []
- msg = self._receive_can()
- data.extend(msg.data[3:])
- while msg is not None and len(data) < n:
- msg = self._receive_can()
+ messages_num = math.ceil(n / 5)
+ data_to_look_for = [2 + n, 0xB4]
+ if data_to_look_for[0] > 7:
+ data_to_look_for[0] = 7
+ msg = self._receive_can(
+ msg_data=data_to_look_for, destroy_can=False, messages_num=messages_num
+ )
+ if messages_num == 1:
data.extend(msg.data[3:])
+ else:
+ for i in range(len(msg)):
+ data.extend(msg[i].data[3:])
return data
def tool_serial_write_data(self, bytes):
@@ -2896,7 +3075,7 @@ def tool_serial_write_data(self, bytes):
msg_bytes.extend(list(chunk))
# print("msg_bytes = " + str(list(msg_bytes)))
self._send_can(msg_bytes)
- msg = self._receive_can()
+ msg = self._receive_can(destroy_can=False)
return msg.data[2]
def tool_serial_flush(self):
diff --git a/pymycobot/mycobotsocket.py b/pymycobot/mycobotsocket.py
index 3c9c19d..aacc809 100644
--- a/pymycobot/mycobotsocket.py
+++ b/pymycobot/mycobotsocket.py
@@ -4,6 +4,7 @@
import time
import math
import socket
+import logging
import threading
from pymycobot.generate import CommandGenerator
@@ -78,7 +79,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyCobotSocket, self)._mesg(genre, *args, **kwargs)
# [254,...,255]
with self.lock:
@@ -189,24 +190,24 @@ def sync_send_angles(self, degrees, speed, timeout=15):
self.send_angles(degrees, speed)
while time.time() - t < timeout:
f = self.is_in_position(degrees, 0)
- if f:
- break
+ if f == 1:
+ return 1
time.sleep(0.1)
- return self
+ return 0
def sync_send_coords(self, coords, speed, mode, timeout=15):
t = time.time()
self.send_coords(coords, speed, mode)
while time.time() - t < timeout:
- if self.is_in_position(coords, 1):
- break
+ if self.is_in_position(coords, 1) == 1:
+ return 1
time.sleep(0.1)
- return self
+ return 0
def set_gpio_mode(self, mode):
"""Set pin coding method
Args:
- mode: (str) BCM or BOARD
+ mode: (str) BCM or BOARD
"""
self.calibration_parameters(gpiomode=mode)
if mode == "BCM":
@@ -244,6 +245,16 @@ def get_gpio_in(self, pin_no):
def wait(self, t):
time.sleep(t)
return self
-
+
def close(self):
self.sock.close()
+
+ def open(self):
+ # 关闭之后需要重新连接
+ self.sock = self.connect_socket()
+ # self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ # print("====open=",self.sock)
+ # self.sock.connect((self.SERVER_IP, self.SERVER_PORT))
+
+ def go_home(self):
+ self.sync_send_angles([0,0,0,0,0,0], 30)
diff --git a/pymycobot/mypalletizer.py b/pymycobot/mypalletizer.py
index 21cd3a4..3e299aa 100644
--- a/pymycobot/mypalletizer.py
+++ b/pymycobot/mypalletizer.py
@@ -3,11 +3,12 @@
import threading
import math
import time
+import threading
from .log import setup_logging
from .generate import CommandGenerator
from .common import ProtocolCode, read, write
-
+from pymycobot.sms import sms_sts
class MyPalletizedataException(Exception):
pass
@@ -80,7 +81,7 @@ def calibration_parameters(**kwargs):
)
-class MyPalletizer(CommandGenerator):
+class MyPalletizer(CommandGenerator, sms_sts):
def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
"""
Args:
@@ -101,6 +102,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
self._serial_port.rts = False
self._serial_port.open()
self.lock = threading.Lock()
+ super(sms_sts, self).__init__(self._serial_port, 0)
_write = write
_read = read
diff --git a/pymycobot/mypalletizer260.py b/pymycobot/mypalletizer260.py
index 7125776..f3297b3 100644
--- a/pymycobot/mypalletizer260.py
+++ b/pymycobot/mypalletizer260.py
@@ -117,7 +117,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(MyPalletizer260, self)._mesg(
+ real_command, has_reply, _async = super(MyPalletizer260, self)._mesg(
genre, *args, **kwargs
)
with self.lock:
@@ -126,7 +126,7 @@ def _mesg(self, genre, *args, **kwargs):
if has_reply:
data = self._read(genre)
res = self._process_received(data, genre)
- if res == []:
+ if res is None:
return None
if genre in [
ProtocolCode.IS_POWER_ON,
diff --git a/pymycobot/mypalletizersocket.py b/pymycobot/mypalletizersocket.py
index 7f9d5b6..0c81ad1 100644
--- a/pymycobot/mypalletizersocket.py
+++ b/pymycobot/mypalletizersocket.py
@@ -4,15 +4,16 @@
import time
import math
import socket
+import logging
import threading
from pymycobot.log import setup_logging
from pymycobot.generate import CommandGenerator
from pymycobot.common import ProtocolCode, write, read
from pymycobot.error import calibration_parameters
+from pymycobot.sms import sms_sts
-
-class MyPalletizerSocket(CommandGenerator):
+class MyPalletizerSocket(CommandGenerator, sms_sts):
"""MyCobot Python API Serial communication class.
Note: Please use this class under the same network
@@ -81,7 +82,7 @@ def _mesg(self, genre, *args, **kwargs):
**kwargs: support `has_reply`
has_reply: Whether there is a return value to accept.
"""
- real_command, has_reply = super(
+ real_command, has_reply, _async = super(
MyPalletizerSocket, self)._mesg(genre, *args, **kwargs)
# [254,...,255]
with self.lock:
@@ -91,7 +92,7 @@ def _mesg(self, genre, *args, **kwargs):
if genre == ProtocolCode.SET_SSID_PWD:
return None
res = self._process_received(data, genre)
- if res == []:
+ if res is None:
return None
if genre in [
ProtocolCode.ROBOT_VERSION,
diff --git a/pymycobot/pro400.py b/pymycobot/pro400.py
index ee97c36..1a7638d 100644
--- a/pymycobot/pro400.py
+++ b/pymycobot/pro400.py
@@ -4,12 +4,11 @@
import serial
from pymycobot.close_loop import CloseLoop
-from pymycobot.error import calibration_parameters
from pymycobot.common import ProtocolCode
class Pro400(CloseLoop):
- def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
+ def __init__(self, port="/dev/ttyAMA1", baudrate="115200", timeout=0.1, debug=False, save_serial_log=False):
"""
Args:
port : port string
@@ -18,7 +17,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
debug : whether show debug info
"""
super(Pro400, self).__init__(debug)
- self.calibration_parameters = calibration_parameters
+ self.save_serial_log = save_serial_log
self._serial_port = serial.Serial()
self._serial_port.port = port
self._serial_port.baudrate = baudrate
@@ -26,8 +25,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
self._serial_port.rts = False
self._serial_port.open()
self.lock = threading.Lock()
- self.has_reply_command = []
- self.is_stop = False
+ self.lock_out = threading.Lock()
self.read_threading = threading.Thread(target=self.read_thread)
self.read_threading.daemon = True
self.read_threading.start()
@@ -177,7 +175,7 @@ def _mesg(self, genre, *args, **kwargs):
]:
return self._process_single(res)
elif genre in [ProtocolCode.GET_ANGLES]:
- return [self._int2angle(angle) for angle in res]
+ return [self._int3angle(angle) for angle in res]
elif genre in [
ProtocolCode.GET_COORDS,
ProtocolCode.MERCURY_GET_BASE_COORDS,
@@ -256,13 +254,6 @@ def open(self):
def close(self):
self._serial_port.close()
-
- def power_on(self, delay=2):
- return super(Pro400, self).power_on()
-
- def power_off(self):
- res = super(Pro400, self).power_off()
- return res
# def power_on_only(self):
# import RPi.GPIO as GPIO
@@ -316,11 +307,6 @@ def power_off(self):
# pin_no = 23
# GPIO.setup(pin_no, GPIO.IN)
# return GPIO.input(pin_no)
-
- def send_angles_sync(self, angles, speed):
- self.calibration_parameters(class_name = self.__class__.__name__, angles=angles, speed=speed)
- angles = [self._angle2int(angle) for angle in angles]
- return self._mesg(ProtocolCode.SEND_ANGLES, angles, speed, no_return=True)
def set_pos_switch(self, mode):
"""Set position switch mode.
diff --git a/pymycobot/pro400client.py b/pymycobot/pro400client.py
index cc8477d..07e2cef 100644
--- a/pymycobot/pro400client.py
+++ b/pymycobot/pro400client.py
@@ -5,7 +5,6 @@
import socket
from pymycobot.close_loop import CloseLoop
-from pymycobot.error import calibration_parameters
from pymycobot.common import ProtocolCode
@@ -19,7 +18,6 @@ def __init__(self, ip, netport=9000, debug=False):
debug : whether show debug info
"""
super(Pro400Client, self).__init__(debug)
- self.calibration_parameters = calibration_parameters
self.SERVER_IP = ip
self.SERVER_PORT = netport
self.sock = self.connect_socket()
@@ -258,40 +256,7 @@ def open(self):
def close(self):
self.sock.close()
-
- def power_on(self):
- return super(Pro400Client, self).power_on()
-
- def power_off(self):
- res = super(Pro400Client, self).power_off()
- return res
-
- def set_basic_output(self, pin_no, pin_signal):
- """Set basic output.IO low-level output high-level, high-level output high resistance state
-
- Args:
- pin_no: pin port number. range 1 ~ 6
- pin_signal: 0 / 1
- """
- return super(Pro400Client, self).set_basic_output(pin_no, pin_signal)
- def get_basic_input(self, pin_no):
- """Get basic input.
-
- Args:
- pin_no: pin port number. range 1 ~ 6
-
- Return:
- 1 - high
- 0 - low
- """
- return super(Pro400Client, self).get_basic_input(pin_no)
-
- def send_angles_sync(self, angles, speed):
- self.calibration_parameters(class_name = self.__class__.__name__, angles=angles, speed=speed)
- angles = [self._angle2int(angle) for angle in angles]
- return self._mesg(ProtocolCode.SEND_ANGLES, angles, speed, no_return=True)
-
def set_pos_switch(self, mode):
"""Set position switch mode.
diff --git a/pymycobot/pro630.py b/pymycobot/pro630.py
index 5e7e906..6533054 100644
--- a/pymycobot/pro630.py
+++ b/pymycobot/pro630.py
@@ -1,7 +1,7 @@
# coding=utf-8
-import time
import threading
+import serial
from pymycobot.close_loop import CloseLoop
from pymycobot.error import calibration_parameters
@@ -9,7 +9,7 @@
class Pro630(CloseLoop):
- def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
+ def __init__(self, port="/dev/ttyAMA1", baudrate="115200", timeout=0.1, debug=False, save_serial_log=False):
"""
Args:
port : port string
@@ -18,8 +18,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
debug : whether show debug info
"""
super(Pro630, self).__init__(debug)
- self.calibration_parameters = calibration_parameters
- import serial
+ # self.calibration_parameters = calibration_parameters
# import RPi.GPIO as GPIO
# self.power_control_1 = 3
# self.power_control_2 = 4
@@ -27,6 +26,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
# GPIO.setwarnings(False)
# GPIO.setup(self.power_control_1, GPIO.IN)
# GPIO.setup(self.power_control_2, GPIO.OUT)
+ self.save_serial_log = save_serial_log
self._serial_port = serial.Serial()
self._serial_port.port = port
self._serial_port.baudrate = baudrate
@@ -34,8 +34,7 @@ def __init__(self, port, baudrate="115200", timeout=0.1, debug=False):
self._serial_port.rts = False
self._serial_port.open()
self.lock = threading.Lock()
- self.has_reply_command = []
- self.is_stop = False
+ self.lock_out = threading.Lock()
self.read_threading = threading.Thread(target=self.read_thread)
self.read_threading.daemon = True
self.read_threading.start()
diff --git a/pymycobot/pro630client.py b/pymycobot/pro630client.py
index d33b34f..095ac5e 100644
--- a/pymycobot/pro630client.py
+++ b/pymycobot/pro630client.py
@@ -20,11 +20,11 @@ def __init__(self, ip, netport=9000, debug=False):
"""
super(Pro630Client, self).__init__(debug)
self.calibration_parameters = calibration_parameters
- self.calibration_parameters = calibration_parameters
self.SERVER_IP = ip
self.SERVER_PORT = netport
self.sock = self.connect_socket()
self.lock = threading.Lock()
+ self.lock_out = threading.Lock()
self.is_stop = False
self.read_threading = threading.Thread(target=self.read_thread, args=("socket",))
self.read_threading.daemon = True
@@ -303,4 +303,4 @@ def get_pos_switch(self):
Return:
1 - switch on, 0 - switch off
"""
- return self._mesg(ProtocolCode.GET_POS_SWITCH, has_reply=True)
\ No newline at end of file
+ return self._mesg(ProtocolCode.GET_POS_SWITCH, has_reply=True)
diff --git a/pymycobot/protocol_packet_handler.py b/pymycobot/protocol_packet_handler.py
new file mode 100644
index 0000000..a58b19b
--- /dev/null
+++ b/pymycobot/protocol_packet_handler.py
@@ -0,0 +1,295 @@
+import time
+
+TXPACKET_MAX_LEN = 250
+RXPACKET_MAX_LEN = 250
+
+# for Protocol Packet
+PKT_HEADER0 = 0
+PKT_HEADER1 = 1
+PKT_ID = 2
+PKT_LENGTH = 3
+PKT_INSTRUCTION = 4
+PKT_ERROR = 4
+PKT_PARAMETER0 = 5
+
+# Protocol Error bit
+ERRBIT_VOLTAGE = 1
+ERRBIT_ANGLE = 2
+ERRBIT_OVERHEAT = 4
+ERRBIT_OVERELE = 8
+ERRBIT_OVERLOAD = 32
+
+BROADCAST_ID = 0xFE # 254
+MAX_ID = 0xFC # 252
+SCS_END = 0
+
+# Instruction for SCS Protocol
+INST_PING = 1
+INST_READ = 2
+INST_WRITE = 3
+INST_REG_WRITE = 4
+INST_ACTION = 5
+INST_SYNC_WRITE = 131 # 0x83
+INST_SYNC_READ = 130 # 0x82
+
+# Communication Result
+COMM_SUCCESS = 0 # tx or rx packet communication success
+COMM_PORT_BUSY = -1 # Port is busy (in use)
+COMM_TX_FAIL = -2 # Failed transmit instruction packet
+COMM_RX_FAIL = -3 # Failed get status packet
+COMM_TX_ERROR = -4 # Incorrect instruction packet
+COMM_RX_WAITING = -5 # Now recieving status packet
+COMM_RX_TIMEOUT = -6 # There is no status packet
+COMM_RX_CORRUPT = -7 # Incorrect status packet
+COMM_NOT_AVAILABLE = -9 #
+
+
+
+class protocol_packet_handler(object):
+ def __init__(self, portHandler, protocol_end):
+ #self.scs_setend(protocol_end)# SCServo bit end(STS/SMS=0, SCS=1)
+ self.portHandler = portHandler
+ self.scs_end = protocol_end
+
+ def save_command(self):
+ save_command = [255, 255, 254, 4, 3, 55, 0]
+ save_command.append((sum(save_command[2:]) & 0xFF) ^ 0xFF)
+ self.portHandler.write(save_command)
+ self.portHandler.flush()
+
+
+ def txPacket(self, txpacket):
+ checksum = 0
+ total_packet_length = txpacket[PKT_LENGTH] + 4 # 4: HEADER0 HEADER1 ID LENGTH
+
+ # if self.portHandler.is_using:
+ # return COMM_PORT_BUSY
+ # self.portHandler.is_using = True
+
+ # # check max packet length
+ # if total_packet_length > TXPACKET_MAX_LEN:
+ # self.portHandler.is_using = False
+ # return COMM_TX_ERROR
+
+ # make packet header
+ txpacket[PKT_HEADER0] = 0xFF
+ txpacket[PKT_HEADER1] = 0xFF
+
+ # add a checksum to the packet
+ for idx in range(2, total_packet_length - 1): # except header, checksum
+ checksum += txpacket[idx]
+
+ txpacket[total_packet_length - 1] = ~checksum & 0xFF
+
+ #print "[TxPacket] %r" % txpacket
+
+ # tx packet
+ self.portHandler.flush()
+ written_packet_length = self.portHandler.write(txpacket)
+ # if total_packet_length != written_packet_length:
+ # self.portHandler.is_using = False
+ # return COMM_TX_FAIL
+
+ return COMM_SUCCESS
+
+ def txRxPacket(self, txpacket):
+ rxpacket = None
+ error = 0
+
+ # tx packet
+ result = self.txPacket(txpacket)
+ if result != COMM_SUCCESS:
+ return rxpacket, result, error
+
+ # (ID == Broadcast ID) == no need to wait for status packet or not available
+ # if (txpacket[PKT_ID] == BROADCAST_ID):
+ # self.portHandler.is_using = False
+ # return rxpacket, result, error
+
+ # set packet timeout
+ # if txpacket[PKT_INSTRUCTION] == INST_READ:
+ # self.portHandler.setPacketTimeout(txpacket[PKT_PARAMETER0 + 1] + 6)
+ # else:
+ # self.portHandler.setPacketTimeout(6) # HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM
+
+ # rx packet
+ result = -1
+ while True:
+ rxpacket, result = self.rxPacket()
+ if result != COMM_SUCCESS or txpacket[PKT_ID] == rxpacket[PKT_ID]:
+ break
+
+ if result == COMM_SUCCESS and txpacket[PKT_ID] == rxpacket[PKT_ID]:
+ error = rxpacket[PKT_ERROR]
+
+ return rxpacket, result, error
+
+ def rxPacket(self):
+ rxpacket = []
+
+ result = COMM_TX_FAIL
+ checksum = 0
+ rx_length = 0
+ wait_length = 6 # minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM)
+ t = time.time()
+ while time.time() - t < 0.1:
+ rxpacket.extend(self.portHandler.read(wait_length - rx_length))
+ rx_length = len(rxpacket)
+ if rx_length >= wait_length:
+ # find packet header
+ for idx in range(0, (rx_length - 1)):
+ if (rxpacket[idx] == 0xFF) and (rxpacket[idx + 1] == 0xFF):
+ break
+
+ if idx == 0: # found at the beginning of the packet
+ if (rxpacket[PKT_ID] > 0xFD) or (rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) or (
+ rxpacket[PKT_ERROR] > 0x7F):
+ # unavailable ID or unavailable Length or unavailable Error
+ # remove the first byte in the packet
+ del rxpacket[0]
+ rx_length -= 1
+ continue
+
+ # re-calculate the exact length of the rx packet
+ if wait_length != (rxpacket[PKT_LENGTH] + PKT_LENGTH + 1):
+ wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1
+ continue
+
+ # if rx_length < wait_length:
+ # # check timeout
+ # if self.portHandler.isPacketTimeout():
+ # if rx_length == 0:
+ # result = COMM_RX_TIMEOUT
+ # else:
+ # result = COMM_RX_CORRUPT
+ # break
+ # else:
+ # continue
+
+ # calculate checksum
+ for i in range(2, wait_length - 1): # except header, checksum
+ checksum += rxpacket[i]
+ checksum = ~checksum & 0xFF
+
+ # verify checksum
+ if rxpacket[wait_length - 1] == checksum:
+ result = COMM_SUCCESS
+ else:
+ result = COMM_RX_CORRUPT
+ break
+
+ else:
+ # remove unnecessary packets
+ del rxpacket[0: idx]
+ rx_length -= idx
+
+ # else:
+ # # check timeout
+ # if self.portHandler.isPacketTimeout():
+ # if rx_length == 0:
+ # result = COMM_RX_TIMEOUT
+ # else:
+ # result = COMM_RX_CORRUPT
+ # break
+ else:
+ None, None
+ # self.portHandler.is_using = False
+ return rxpacket, result
+
+ def readTxRx(self, scs_id, address, length):
+ txpacket = [0] * 8
+ data = []
+
+ if scs_id >= BROADCAST_ID:
+ return data, COMM_NOT_AVAILABLE, 0
+
+ txpacket[PKT_ID] = scs_id
+ txpacket[PKT_LENGTH] = 4
+ txpacket[PKT_INSTRUCTION] = INST_READ
+ txpacket[PKT_PARAMETER0 + 0] = address
+ txpacket[PKT_PARAMETER0 + 1] = length
+
+ rxpacket, result, error = self.txRxPacket(txpacket)
+ if result == COMM_SUCCESS:
+ error = rxpacket[PKT_ERROR]
+
+ data.extend(rxpacket[PKT_PARAMETER0 : PKT_PARAMETER0+length])
+
+ return data, result, error
+
+ def read1ByteTxRx(self, scs_id, address):
+ data, result, error = self.readTxRx(scs_id, address, 1)
+ data_read = data[0] if (result == COMM_SUCCESS) else 0
+ return data_read, result, error
+
+ def read2ByteTxRx(self, scs_id, address):
+ data, result, error = self.readTxRx(scs_id, address, 2)
+ data_read = self.scs_makeword(data[0], data[1]) if (result == COMM_SUCCESS) else 0
+ return data_read, result, error
+
+ def scs_makeword(self,a, b):
+ if self.scs_end==0:
+ return (a & 0xFF) | ((b & 0xFF) << 8)
+ else:
+ return (b & 0xFF) | ((a & 0xFF) << 8)
+ def scs_tohost(self, a, b):
+ if (a & (1<<b)):
+ return -(a & ~(1<<b))
+ else:
+ return a
+
+ def write1ByteTxRx(self, scs_id, address, data):
+ data_write = [data]
+ return self.writeTxRx(scs_id, address, 1, data_write)
+
+ def writeTxRx(self, scs_id, address, length, data):
+ if address in [6,7,18,13,14,21,22,23,26,27,81,24]:
+ self.save_command()
+ txpacket = [0] * (length + 7)
+
+ txpacket[PKT_ID] = scs_id
+ txpacket[PKT_LENGTH] = length + 3
+ txpacket[PKT_INSTRUCTION] = INST_WRITE
+ txpacket[PKT_PARAMETER0] = address
+
+ txpacket[PKT_PARAMETER0 + 1: PKT_PARAMETER0 + 1 + length] = data[0: length]
+ rxpacket, result, error = self.txRxPacket(txpacket)
+
+ return result, error
+
+ def write2ByteTxRx(self, scs_id, address, data):
+ data_write = [self.scs_lobyte(data), self.scs_hibyte(data)]
+ return self.writeTxRx(scs_id, address, 2, data_write)
+
+ def scs_lobyte(self, w):
+ if self.scs_end==0:
+ return w & 0xFF
+ else:
+ return (w >> 8) & 0xFF
+
+ def scs_hibyte(self, w):
+ if self.scs_end==0:
+ return (w >> 8) & 0xFF
+ else:
+ return w & 0xFF
+
+ def ping(self, scs_id):
+ model_number = 0
+ error = 0
+
+ txpacket = [0] * 6
+
+ if scs_id >= BROADCAST_ID:
+ return model_number, COMM_NOT_AVAILABLE, error
+
+ txpacket[PKT_ID] = scs_id
+ txpacket[PKT_LENGTH] = 2
+ txpacket[PKT_INSTRUCTION] = INST_PING
+
+ rxpacket, result, error = self.txRxPacket(txpacket)
+ if result == COMM_SUCCESS:
+ data_read, result, error = self.readTxRx(scs_id, 3, 2) # Address 3 : Model Number
+ if result == COMM_SUCCESS:
+ model_number = self.scs_makeword(data_read[0], data_read[1])
+
+ return result
\ No newline at end of file
diff --git a/pymycobot/robot_info.py b/pymycobot/robot_info.py
index df95583..615603d 100644
--- a/pymycobot/robot_info.py
+++ b/pymycobot/robot_info.py
@@ -1,4 +1,4 @@
-# coding=utf-8
+ # coding=utf-8
class Robot320Info(object):
error_info = {
@@ -56,3 +56,348 @@ class Robot320Info(object):
}
}
+# coding=utf-8
+
+def _interpret_status_code(language, status_code):
+ # 将状态码映射到易懂的提示信息
+ status_messages = {
+ "zh_CN": {
+ 0: "",
+ 1: "错误：关节1临近限位，",
+ 2: "错误：关节2临近限位，",
+ 3: "错误：关节3临近限位，",
+ 4: "错误：关节4临近限位，",
+ 5: "错误：关节5临近限位，",
+ 6: "错误：关节6临近限位，",
+ 7: "错误：关节7临近限位，",
+ 10: "提示：运动缓停结束。",
+ 11: "提示：运动急停结束。",
+ 32: "错误：坐标无解，请检查机器人手臂跨度是否接近极限。",
+ 33: "错误：直线运动无相邻解。请检查机器人手臂跨度是否接近极限。",
+ 34: "错误：速度融合错误。请检查机器人手臂跨度是否接近极限。",
+ 35: "错误：零空间运动无相邻解。请检查机器人手臂跨度是否接近极限。",
+ 36: "错误：奇异位置无解。请使用`send_angles()`方法离开该位置。",
+ 49: "错误：掉使能。请使用 `power_on()` 方法重新上电。",
+ 50: "错误：电机报错。",
+ 51: "错误：电机编码器报错。",
+ 52: "错误：电机执行异常。",
+ 53: "错误：关节位置超差。",
+ 64: "错误: 发送的坐标数据超出限位.",
+ 65: "错误：关节1位置精度异常。",
+ 66: "错误：关节2位置精度异常。",
+ 67: "错误：关节3位置精度异常。",
+ 68: "错误：关节4位置精度异常。",
+ 69: "错误：关节5位置精度异常。",
+ 70: "错误：关节6位置精度异常。",
+ 71: "错误：关节7位置精度异常。",
+ 81: "错误：关节1碰撞检测异常。",
+ 82: "错误：关节2碰撞检测异常。",
+ 83: "错误：关节3碰撞检测异常。",
+ 84: "错误：关节4碰撞检测异常。",
+ 85: "错误：关节5碰撞检测异常。",
+ 86: "错误：关节6碰撞检测异常。",
+ 87: "错误：关节7碰撞检测异常。",
+ 97: "错误：关节1 can发送失败。",
+ 98: "错误：关节2 can发送失败。",
+ 99: "错误：关节3 can发送失败。",
+ 100: "错误：关节4 can发送失败。",
+ 101: "错误：关节5 can发送失败。",
+ 102: "错误：关节6 can发送失败。",
+ 103: "错误：关节7 can发送失败。",
+ 113: "错误：关节1 can接收异常。",
+ 114: "错误：关节2 can接收异常。",
+ 115: "错误：关节3 can接收异常。",
+ 116: "错误：关节4 can接收异常。",
+ 117: "错误：关节5 can接收异常。",
+ 118: "错误：关节6 can接收异常。",
+ 119: "错误：关节7 can接收异常。",
+ 129: "错误：关节1掉使能。",
+ 130: "错误：关节2掉使能。",
+ 131: "错误：关节3掉使能。",
+ 132: "错误：关节4掉使能。",
+ 133: "错误：关节5掉使能。",
+ 134: "错误：关节6掉使能。",
+ 135: "错误：关节7掉使能。",
+ 145: "错误：关节1电机报错。可以使用get_motors_run_err()接口获取详细错误码。",
+ 146: "错误：关节2电机报错。可以使用get_motors_run_err()接口获取详细错误码。",
+ 147: "错误：关节3电机报错。可以使用get_motors_run_err()接口获取详细错误码。",
+ 148: "错误：关节4电机报错。可以使用get_motors_run_err()接口获取详细错误码。",
+ 149: "错误：关节5电机报错。可以使用get_motors_run_err()接口获取详细错误码。",
+ 150: "错误：关节6电机报错。可以使用get_motors_run_err()接口获取详细错误码。",
+ 151: "错误：关节7电机报错。可以使用get_motors_run_err()接口获取详细错误码。",
+ 161: "错误：关节1电机编码器报错。",
+ 162: "错误：关节2电机编码器报错。",
+ 163: "错误：关节3电机编码器报错。",
+ 164: "错误：关节4电机编码器报错。",
+ 165: "错误：关节5电机编码器报错。",
+ 166: "错误：关节6电机编码器报错。",
+ 167: "错误：关节7电机编码器报错。",
+ 193: "错误：关节1电机位置超差。",
+ 194: "错误：关节2电机位置超差。",
+ 195: "错误：关节3电机位置超差。",
+ 196: "错误：关节4电机位置超差。",
+ 197: "错误：关节5电机位置超差。",
+ 198: "错误：关节6电机位置超差。",
+ 199: "错误：关节7电机位置超差。",
+ 255: "错误：未知错误代码: "
+ },
+ "en_US": {
+ 0: "",
+ 1: "Error: Joint 1 proximity limit.",
+ 2: "Error: Joint 2 proximity limit.",
+ 3: "Error: Joint 3 proximity limit.",
+ 4: "Error: Joint 4 proximity limit.",
+ 5: "Error: Joint 5 proximity limit.",
+ 6: "Error: Joint 6 proximity limit.",
+ 7: "Error: Joint 7 proximity limit.",
+ 10: "Tip: Motion slow stop is over.",
+ 11: "Tip: Motion emergency stop is over.",
+ 32: "ERROR: Invkinematics no solution, please check if the robot arm span approach limit.",
+ 33: "Error: Straight-line motion has no adjacent solution. please check if the robot arm span approach limit",
+ 34: "Error: Velocity fusion error. please check if the robot arm span approach limit",
+ 35: "Error: Zero space motion has no adjacent solution. please check if the robot arm span approach limit",
+ 36: "Error: Singular position has no solution. please move away by `send_angles()`",
+ 49: "Error: Power failure. Please use the `power_on()` method to re-power on.",
+ 50: "Error: Motor error.",
+ 51: "Error: Motor encoder error.",
+ 52: "Error: Motor execution exception.",
+ 53: "Error：The joint position is out of tolerance.",
+ 64: "Error: The sent coordinates exceed the limit.",
+ 65: "Error: Joint 1 position precision exception.",
+ 66: "Error: Joint 2 position precision exception.",
+ 67: "Error: Joint 3 position precision exception.",
+ 68: "Error: Joint 4 position precision exception.",
+ 69: "Error: Joint 5 position precision exception.",
+ 70: "Error: Joint 6 position precision exception.",
+ 71: "Error: Joint 7 position precision exception.",
+ 81: "Error: Joint 1 collision detection exception.",
+ 82: "Error: Joint 2 collision detection exception.",
+ 83: "Error: Joint 3 collision detection exception.",
+ 84: "Error: Joint 4 collision detection exception.",
+ 85: "Error: Joint 5 collision detection exception.",
+ 86: "Error: Joint 6 collision detection exception.",
+ 87: "Error: Joint 7 collision detection exception.",
+ 97: "Error: Joint 1 can fail to send.",
+ 98: "Error: Joint 2 can fail to send.",
+ 99: "Error: Joint 3 can fail to send.",
+ 100: "Error: Joint 4 can fail to send.",
+ 101: "Error: Joint 5 can fail to send.",
+ 102: "Error: Joint 6 can fail to send.",
+ 103: "Error: Joint 7 can fail to send.",
+ 113: "Error: Joint 1 can receive abnormality.",
+ 114: "Error: Joint 2 can receive abnormality.",
+ 115: "Error: Joint 3 can receive abnormality.",
+ 116: "Error: Joint 4 can receive abnormality.",
+ 117: "Error: Joint 5 can receive abnormality.",
+ 118: "Error: Joint 6 can receive abnormality.",
+ 119: "Error: Joint 7 can receive abnormality.",
+ 129: "Error: Joint 1 is disabled.",
+ 130: "Error: Joint 2 is not enabled.",
+ 131: "Error: Joint 3 is not enabled.",
+ 132: "Error: Joint 4 is not enabled.",
+ 133: "Error: Joint 5 is not enabled.",
+ 134: "Error: Joint 6 is not enabled.",
+ 135: "Error: Joint 7 is not enabled.",
+ 145: "Error: Joint 1 motor reports an error. You can use the get_motors_run_err() interface to get the detailed error code.",
+ 146: "Error: Joint 2 motor reports an error. You can use the get_motors_run_err() interface to get the detailed error code.",
+ 147: "Error: Joint 3 motor reports an error. You can use the get_motors_run_err() interface to get the detailed error code.",
+ 148: "Error: Joint 4 motor reports an error. You can use the get_motors_run_err() interface to get the detailed error code.",
+ 149: "Error: Joint 5 motor reported an error. You can use the get_motors_run_err() interface to get the detailed error code.",
+ 150: "Error: Joint 6 motor reported an error. You can use the get_motors_run_err() interface to get the detailed error code.",
+ 151: "Error: Joint 7 motor reported an error. You can use the get_motors_run_err() interface to get the detailed error code.",
+ 161: "Error: Joint 1 motor encoder reported an error.",
+ 162: "Error: Joint 2 motor encoder reported an error.",
+ 163: "Error: Joint 3 motor encoder reported an error.",
+ 164: "Error: Joint 4 motor encoder reported an error.",
+ 165: "Error: Joint 5 motor encoder reported an error.",
+ 166: "Error: Joint 6 motor encoder reported an error.",
+ 167: "Error: Joint 7 motor encoder reported an error.",
+ 193: "Error: Joint 1 motor position is out of tolerance. ",
+ 194: "Error: Joint 2 motor position is out of tolerance. ",
+ 195: "Error: Joint 3 motor position is out of tolerance. ",
+ 196: "Error: Joint 4 motor position is out of tolerance. ",
+ 197: "Error: Joint 5 motor position is out of tolerance. ",
+ 198: "Error: Joint 6 motor position is out of tolerance. ",
+ 199: "Error: Joint 7 motor position is out of tolerance. ",
+ 255: "Error: Unknown error code: "
+ }
+ }
+ return status_messages[language].get(status_code, status_messages[language].get(255)+str(status_code))
+
+class RobotLimit:
+ robot_limit = {
+ "Mercury":{
+ "joint_id":[1,2,3,4,5,6,7,11,12,13],
+ "angles_min":[-165, -50, -165, -165, -165, -20, -180, -60, -138, -115],
+ "angles_max":[165, 120, 165, 5, 165, 265, 180, 0, 175, 115],
+ "coords_min":[-459, -459, -300, -180, -180, -180],
+ "coords_max":[459, 459, 542, 180, 180, 180],
+ "left_coords_min":[-351.11, -272.12, -262.91, -180, -180, -180],
+ "left_coords_max":[566.92, 645.91, 655.13, 180, 180, 180],
+ "right_coords_min":[-351.11, -645.91, -262.91, -180, -180, -180],
+ "right_coords_max":[566.92, 272.12, 655.13, 180, 180, 180]
+ },
+ "MercurySocket":{
+ "joint_id":[1,2,3,4,5,6,7,11,12,13],
+ "angles_min":[-165, -50, -165, -165, -165, -20, -180, -60, -138, -115],
+ "angles_max":[165, 120, 165, 5, 165, 265, 180, 0, 175, 115],
+ "coords_min":[-459, -459, -300, -180, -180, -180],
+ "coords_max":[459, 459, 542, 180, 180, 180],
+ "left_coords_min":[-351.11, -272.12, -262.91, -180, -180, -180],
+ "left_coords_max":[566.92, 645.91, 655.13, 180, 180, 180],
+ "right_coords_min":[-351.11, -645.91, -262.91, -180, -180, -180],
+ "right_coords_max":[566.92, 272.12, 655.13, 180, 180, 180]
+ },
+ "MyCobot": {
+ "id": [1, 2, 3, 4, 5, 6, 7],
+ "angles_min": [-168, -135, -150, -145, -165, -180],
+ "angles_max": [168, 135, 150, 145, 165, 180],
+ "coords_min": [-350, -350, -70, -180, -180, -180],
+ "coords_max": [350, 350, 523.9, 180, 180, 180]
+ },
+ "MyCobotSocket": {
+ "id": [1, 2, 3, 4, 5, 6, 7],
+ "angles_min": [-168, -135, -150, -145, -165, -180],
+ "angles_max": [168, 135, 150, 145, 165, 180],
+ "coords_min": [-350, -350, -70, -180, -180, -180],
+ "coords_max": [350, 350, 523.9, 180, 180, 180]
+ },
+ "MyCobot280": {
+ "id": [1, 2, 3, 4, 5, 6],
+ "angles_min": [-168, -135, -150, -145, -165, -180],
+ "angles_max": [168, 135, 150, 145, 165, 180],
+ "coords_min": [-350, -350, -70, -180, -180, -180],
+ "coords_max": [350, 350, 523.9, 180, 180, 180]
+ },
+ "MyCobot280Socket": {
+ "id": [1, 2, 3, 4, 5, 6],
+ "angles_min": [-168, -135, -150, -145, -165, -180],
+ "angles_max": [168, 135, 150, 145, 165, 180],
+ "coords_min": [-350, -350, -70, -180, -180, -180],
+ "coords_max": [350, 350, 523.9, 180, 180, 180]
+ },
+ "MyCobot320": {
+ "id": [1, 2, 3, 4, 5, 6],
+ "angles_min": [-168, -135, -145, -148, -168, -180],
+ "angles_max": [168, 135, 145, 148, 168, 180],
+ "coords_min": [-350, -350, -41, -180, -180, -180],
+ "coords_max": [350, 350, 523.9, 180, 180, 180]
+ },
+ "MyCobot320Socket": {
+ "id": [1, 2, 3, 4, 5, 6],
+ "angles_min": [-168, -135, -145, -148, -168, -180],
+ "angles_max": [168, 135, 145, 148, 168, 180],
+ "coords_min": [-350, -350, -41, -180, -180, -180],
+ "coords_max": [350, 350, 523.9, 180, 180, 180]
+ },
+ "MechArm": {
+ "id": [1, 2, 3, 4, 5, 6, 7],
+ "angles_min": [-165, -90, -180, -165, -115, -175],
+ "angles_max": [165, 90, 70, 165, 115, 175],
+ "coords_min": [-272, -272, -36, -180, -180, -180],
+ "coords_max": [272, 272, 408.9, 180, 180, 180]
+ },
+ "MechArm270": {
+ "id": [1, 2, 3, 4, 5, 6],
+ "angles_min": [-160, -75, -175, -155, -115, -180],
+ "angles_max": [160, 120, 65, 155, 115, 180],
+ "coords_min": [-272, -272, -86, -180, -180, -180],
+ "coords_max": [272, 272, 408.9, 180, 180, 180]
+ },
+ "MechArmSocket": {
+ "id": [1, 2, 3, 4, 5, 6],
+ "angles_min": [-160, -75, -175, -155, -115, -180],
+ "angles_max": [160, 120, 65, 155, 115, 180],
+ "coords_min": [-272, -272, -86, -180, -180, -180],
+ "coords_max": [272, 272, 408.9, 180, 180, 180]
+ },
+ "MyArm": {
+ "id": [1, 2, 3, 4, 5, 6, 7, 8],
+ "angles_min": [-160, -70, -170, -113, -170, -115, -180],
+ "angles_max": [160, 115, 170, 75, 170, 115, 180],
+ "coords_min": [-310, -310, -140, -180, -180, -180],
+ "coords_max": [310, 310, 480, 180, 180, 180]
+ },
+ "MyArmC": {
+ "joint_id": [1, 2, 3, 4, 5, 6],
+ "servo_id": [1, 2, 3, 4, 5, 6, 7, 8],
+ "angles_min": [-165, -80, -100, -160, -90, -180],
+ "angles_max": [165, 100, 80, 160, 90, 180]
+ },
+ "MyArmM": {
+ "joint_id": [1, 2, 3, 4, 5, 6, 7],
+ "servo_id": [1, 2, 3, 4, 5, 6, 7, 8],
+ "angles_min": [-170, -83, -90, -155, -91, -153, -118],
+ "angles_max": [170, 83, 84, 153, 88, 153, 2],
+ "encoders_min": [137, 1163, 1035, 1013, 248, 979, 220, 706],
+ "encoders_max": [4004, 2945, 3079, 3026, 3724, 2994, 3704, 2048]
+ },
+ "MyArmMControl": {
+ "joint_id": [1, 2, 3, 4, 5, 6],
+ "servo_id": [1, 2, 3, 4, 5, 6, 7, 8],
+ "coord_id": [1, 2, 3, 4, 5, 6],
+ "coord_min": [-833.325, -833.325, -351.11, -180, -180, -180],
+ "coord_max": [833.325, 833.325, 1007.225, 180, 180, 180],
+ "angles_min": [-170, -83, -90, -155, -91, -153, -118],
+ "angles_max": [170, 83, 84, 153, 88, 153, 2],
+ "encoders_min": [137, 1163, 1035, 1013, 248, 979, 220, 706],
+ "encoders_max": [4004, 2945, 3079, 3026, 3724, 2994, 3704, 2048]
+ },
+ "MyArmSocket": {
+ "id": [1, 2, 3, 4, 5, 6, 7, 8],
+ "angles_min": [-160, -70, -170, -113, -170, -115, -180],
+ "angles_max": [160, 115, 170, 75, 170, 115, 180],
+ "coords_min": [-310, -310, -140, -180, -180, -180],
+ "coords_max": [310, 310, 480, 180, 180, 180]
+ },
+ "MyPalletizer": {
+ "id": [1, 2, 3, 4, 7],
+ "angles_min": [-162, -2, -92, -180],
+ "angles_max": [162, 90, 60, 180],
+ "coords_min": [-260, -260, -15, -180],
+ "coords_max": [260, 260, 357.58, 180]
+ },
+ "MyPalletizer260": {
+ "id": [1, 2, 3, 4],
+ "angles_min": [-162, -2, -92, -180],
+ "angles_max": [162, 90, 60, 180],
+ "coords_min": [-260, -260, -15, -180],
+ "coords_max": [260, 260, 357.58, 180]
+ },
+ "MyPalletizerSocket": {
+ "id": [1, 2, 3, 4],
+ "angles_min": [-162, -2, -92, -180],
+ "angles_max": [162, 90, 60, 180],
+ "coords_min": [-260, -260, -15, -180],
+ "coords_max": [260, 260, 357.58, 180]
+ },
+ "UltraArm": {
+ "id": [1, 2, 3, 4, 7],
+ "angles_min": [-150, -20, -5, -179],
+ "angles_max": [170, 90, 110, 179],
+ "coords_min": [-360, -365.55, -140, -180],
+ "coords_max": [365.55, 365.55, 130, 180]
+ },
+ "MyBuddy": {
+ "id": [1, 2, 3, 4, 5, 6, 7],
+ "angles_min": [-165, -165, -165, -165, -165, -175],
+ "angles_max": [165, 165, 165, 165, 165, 175],
+ "waist_angle_min": -120,
+ "waist_angle_max": 120,
+ "left_coords_min": [0, -40, 0, -180, -180, -180],
+ "left_coords_max": [250, 260, 480, 180, 180, 180],
+ "right_coords_min": [0, -260, 0, -180, -180, -180],
+ "right_coords_max": [250, 40, 480, 180, 180, 180]
+ },
+ "MyBuddySocket": {
+ "id": [1, 2, 3, 4, 5, 6, 7],
+ "angles_min": [-165, -165, -165, -165, -165, -175],
+ "angles_max": [165, 165, 165, 165, 165, 175],
+ "waist_angle_min": -120,
+ "waist_angle_max": 120,
+ "left_coords_min": [0, -40, 0, -180, -180, -180],
+ "left_coords_max": [250, 260, 480, 180, 180, 180],
+ "right_coords_min": [0, -260, 0, -180, -180, -180],
+ "right_coords_max": [250, 40, 480, 180, 180, 180]
+ }
+ }
diff --git a/pymycobot/sms.py b/pymycobot/sms.py
new file mode 100644
index 0000000..712b634
--- /dev/null
+++ b/pymycobot/sms.py
@@ -0,0 +1,236 @@
+
+from .protocol_packet_handler import *
+# 1 byte
+FIRMWARE_MAJOR = 0x00
+FIRMWARE_MINOR = 0x01
+SERVO_MAJOR = 0x03
+SERVO_MINOR = 0x04
+BAUD = 6
+RETURN_DELAY = 7
+PHASE = 18
+MAX_TEMPERATURE = 13
+MAX_VOLTAGE = 14
+SERVO_P = 21
+SERVO_D = 22
+SERVO_I = 23
+CLOCKWISE_INSENSITIVE_ZONE = 26
+COUNTERCLOCKWISE_INSENSITIVE_ZONE = 27
+D_CONTROL_TIME = 81
+# 2 bytes
+MIN_START_FORCE = 24
+
+
+class sms_sts(protocol_packet_handler):
+ def __init__(self, portHandler):
+ protocol_packet_handler.__init__(self, portHandler, 0)
+
+ def get_servo_baud(self, id):
+ """获取舵机波特率
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res = self.read1ByteTxRx(id, BAUD)
+ return res[0] if res[1] != -2 else None
+
+ def set_servo_baud(self, id, value):
+ """设置舵机波特率"""
+ return self.write1ByteTxRx(id, BAUD, value)
+
+ def get_servo_response_speed(self, id):
+ """获取舵机响应速度
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res = self.read1ByteTxRx(id, RETURN_DELAY)
+ return res[0] if res[1] != -2 else None
+
+ def set_servo_response_speed(self, id, value):
+ """设置舵机响应速度"""
+ return self.write1ByteTxRx(id, RETURN_DELAY, value)
+
+ def get_servo_phase(self, id):
+ """获取舵机相位
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res = self.read1ByteTxRx(id, PHASE)
+ return res[0] if res[1] != -2 else None
+
+ def set_servo_phase(self, id, value):
+ """设置舵机相位"""
+ return self.write1ByteTxRx(id, PHASE, value)
+
+ def get_servo_max_temperature(self, id):
+ """获取舵机最大温度
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res = self.read1ByteTxRx(id, MAX_TEMPERATURE)
+ return res[0] if res[1] != -2 else None
+
+ def set_servo_max_temperature(self, id, value):
+ """设置舵机最大温度"""
+ return self.write1ByteTxRx(id, MAX_TEMPERATURE, value)
+
+ def get_servo_max_voltage(self, id):
+ """获取舵机最大电压
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res = self.read1ByteTxRx(id, MAX_VOLTAGE)
+ return res[0] if res[1] != -2 else None
+
+ def set_servo_max_voltage(self, id, value):
+ """设置舵机最大电压"""
+ return self.write1ByteTxRx(id, MAX_VOLTAGE, value)
+
+ def get_servo_pid(self, id):
+ """获取舵机pid
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res_p = self.read1ByteTxRx(id, SERVO_P)
+ res_i = self.read1ByteTxRx(id, SERVO_I)
+ res_d = self.read1ByteTxRx(id, SERVO_D)
+ return [res_p[0] if res_p[1] != -2 else None, res_i[0] if res_i[1] != -2 else None, res_d[0] if res_d[1] != -2 else None]
+
+ def set_servo_pid(self, id, pid):
+ """设置舵机pid"""
+ self.write1ByteTxRx(id, SERVO_P, pid[0])
+ self.write1ByteTxRx(id, SERVO_I, pid[1])
+ self.write1ByteTxRx(id, SERVO_D, pid[2])
+
+ def get_servo_clockwise(self, id):
+ """获取舵机顺时针不灵敏区
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res = self.read1ByteTxRx(id, CLOCKWISE_INSENSITIVE_ZONE)
+ return res[0] if res[1] != -2 else None
+
+ def set_servo_clockwise(self, id, value):
+ """设置舵机顺时针不灵敏区"""
+ return self.write1ByteTxRx(id, CLOCKWISE_INSENSITIVE_ZONE, value)
+
+ def get_servo_counter_clockwise(self, id):
+ """获取舵机逆时针不灵敏区
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res = self.read1ByteTxRx(id, COUNTERCLOCKWISE_INSENSITIVE_ZONE)
+ return res[0] if res[1] != -2 else None
+
+ def set_servo_counter_clockwise(self, id, value):
+ """设置舵机逆时针不灵敏区"""
+ return self.write1ByteTxRx(id, COUNTERCLOCKWISE_INSENSITIVE_ZONE, value)
+
+ def get_servo_d_time(self, id):
+ """获取舵机D控制时间
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ res = self.read1ByteTxRx(id, D_CONTROL_TIME)
+ return res[0] if res[1] != -2 else None
+
+ def set_servo_d_time(self, id, value):
+ """设置舵机D控制时间"""
+ return self.write1ByteTxRx(id, D_CONTROL_TIME, value)
+
+ def get_servo_min_start(self, id):
+ """获取舵机最小启动力
+
+ Args:
+ id: 电机Id
+
+ Return:
+ None: 获取失败
+ """
+ scs_present_speed, scs_comm_result, scs_error = self.read2ByteTxRx(id, MIN_START_FORCE)
+ return self.scs_tohost(scs_present_speed, 15) if scs_comm_result != -2 else None
+
+ def set_servo_min_start(self, id, value):
+ """设置舵机最小启动力"""
+ return self.write2ByteTxRx(id, MIN_START_FORCE, value)
+
+ def search_servo(self, id):
+ """搜索舵机是否存在
+
+ Args:
+ id：舵机ID号.
+
+ Return:
+ 0: 存在.
+ -6：不存在.
+ None: 获取失败.
+ """
+ res = self.ping(id)
+ if res == -2:
+ return None
+ return res
+
+ def get_servo_error(self, id):
+ """获取舵机错误信息
+
+ Return:
+ error_info: 将返回的十进制数据转换为二进制
+ Bit0 Bit1 Bit2 Bit3 Bit4 Bit5 对应位被置1表示相应错误出现\n
+ 电压 传感器 温度 电流 角度 过载 对应位0为无相应该错误\n
+ None:表示获取失败
+ """
+ res = self.read1ByteTxRx(id, 0x41)
+ if res[1] == -2:
+ return None
+ return res[0]
+
+ def get_servo_firmware_version(self, id):
+ """获取电机固件版本信息
+
+ Return:
+ list: [固件主版本号, 固件次版本号, 舵机主版本号, 舵机次版本号], -1 表示获取失败
+ """
+ res = []
+ command = [FIRMWARE_MAJOR, FIRMWARE_MINOR, SERVO_MAJOR, SERVO_MINOR]
+ for add in command:
+ r = self.read1ByteTxRx(id, add)
+ if r[1] == -2:
+ res.append(-1)
+ else:
+ res.append(r[0])
+ return res
+
diff --git a/pymycobot/tool_coords.py b/pymycobot/tool_coords.py
new file mode 100644
index 0000000..ccc7b2f
--- /dev/null
+++ b/pymycobot/tool_coords.py
@@ -0,0 +1,92 @@
+# coding=utf-8
+
+import numpy as np
+
+def CvtRotationMatrixToEulerAngle(pdtRotationMatrix):
+ pdtEulerAngle = np.zeros(3)
+
+ pdtEulerAngle[2] = np.arctan2(pdtRotationMatrix[1, 0], pdtRotationMatrix[0, 0])
+
+ fCosRoll = np.cos(pdtEulerAngle[2])
+ fSinRoll = np.sin(pdtEulerAngle[2])
+
+ pdtEulerAngle[1] = np.arctan2(-pdtRotationMatrix[2, 0], (fCosRoll * pdtRotationMatrix[0, 0]) + (fSinRoll * pdtRotationMatrix[1, 0]))
+ pdtEulerAngle[0] = np.arctan2((fSinRoll * pdtRotationMatrix[0, 2]) - (fCosRoll * pdtRotationMatrix[1, 2]), (-fSinRoll * pdtRotationMatrix[0, 1]) + (fCosRoll * pdtRotationMatrix[1, 1]))
+
+ return pdtEulerAngle
+
+def CvtEulerAngleToRotationMatrix(ptrEulerAngle):
+ ptrSinAngle = np.sin(ptrEulerAngle)
+ ptrCosAngle = np.cos(ptrEulerAngle)
+
+ ptrRotationMatrix = np.zeros((3, 3))
+ ptrRotationMatrix[0, 0] = ptrCosAngle[2] * ptrCosAngle[1]
+ ptrRotationMatrix[0, 1] = ptrCosAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] - ptrSinAngle[2] * ptrCosAngle[0]
+ ptrRotationMatrix[0, 2] = ptrCosAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] + ptrSinAngle[2] * ptrSinAngle[0]
+ ptrRotationMatrix[1, 0] = ptrSinAngle[2] * ptrCosAngle[1]
+ ptrRotationMatrix[1, 1] = ptrSinAngle[2] * ptrSinAngle[1] * ptrSinAngle[0] + ptrCosAngle[2] * ptrCosAngle[0]
+ ptrRotationMatrix[1, 2] = ptrSinAngle[2] * ptrSinAngle[1] * ptrCosAngle[0] - ptrCosAngle[2] * ptrSinAngle[0]
+ ptrRotationMatrix[2, 0] = -ptrSinAngle[1]
+ ptrRotationMatrix[2, 1] = ptrCosAngle[1] * ptrSinAngle[0]
+ ptrRotationMatrix[2, 2] = ptrCosAngle[1] * ptrCosAngle[0]
+
+ return ptrRotationMatrix
+
+def transformation_matrix_from_parameters(rotation_matrix, translation_vector):
+ """
+ Create a 4x4 homogeneous transformation matrix.
+
+ :param rotation_matrix: 3x3 numpy array representing rotation.
+ :param translation_vector: 1x3 numpy array representing translation.
+ :return: 4x4 numpy array representing the transformation matrix.
+ """
+ T = np.eye(4)
+ T[:3, :3] = rotation_matrix
+ T[:3, 3] = translation_vector
+ return T
+
+def get_flange_pose(flange_matrix, tool_pose):
+ """
+ Convert the pose from flange coordinate system to tool coordinate system.
+
+ :param flange_to_tool_matrix: 4x4 numpy array, transformation matrix from flange to tool.
+ :param flange_pose: 4x4 numpy array, pose in flange coordinate system.
+ :return: 4x4 numpy array, pose in tool coordinate system.
+ """
+ return np.dot(flange_matrix, np.linalg.inv(tool_pose))
+
+def get_tool_pose(flange_matrix, tool_pose):
+ """
+ Convert the pose from tool coordinate system to flange coordinate system.
+
+ :param flange_to_tool_matrix: 4x4 numpy array, transformation matrix from flange to tool.
+ :param tool_pose: 4x4 numpy array, pose in tool coordinate system.
+ :return: 4x4 numpy array, pose in flange coordinate system.
+ """
+ return np.dot(flange_matrix, tool_pose)
+
+
+def flangeToTool(current_coords, tool_matrix):
+ # Example pose in flange coordinate system
+ flange_pose = np.eye(4)
+ flange_pose[:3, :3] = CvtEulerAngleToRotationMatrix(current_coords[3:6] * np.pi/180.0)
+ flange_pose[:3, 3] = current_coords[:3] # Example translation
+
+ # Switch to tool coordinate system
+ tool_pose = get_tool_pose(flange_pose, tool_matrix)
+ tool_coords = np.concatenate((tool_pose[:3,3].T, CvtRotationMatrixToEulerAngle(tool_pose[:3,:3]) * 180/np.pi))
+
+ return tool_coords
+
+def toolToflange(tool_coords, tool_matrix):
+
+ # Example pose in tool coordinate system
+ tool_pose = np.eye(4)
+ tool_pose[:3, :3] = CvtEulerAngleToRotationMatrix(tool_coords[3:6] * np.pi/180.0)
+ tool_pose[:3, 3] = tool_coords[:3] # Example translation
+
+ # Switch to tool coordinate system
+ flange_pose = get_flange_pose(tool_pose, tool_matrix)
+
+ flange_coords = np.concatenate((flange_pose[:3,3].T, CvtRotationMatrixToEulerAngle(flange_pose[:3,:3]) * 180/np.pi ))
+ return flange_coords
\ No newline at end of file
diff --git a/pymycobot/ultraArm.py b/pymycobot/ultraArm.py
index 8fe2dc4..b0a9adf 100644
--- a/pymycobot/ultraArm.py
+++ b/pymycobot/ultraArm.py
@@ -2,6 +2,7 @@
import time
from pymycobot.common import ProtocolCode
import math
+import threading
class ultraArm:
@@ -24,6 +25,7 @@ def __init__(self, port, baudrate=115200, timeout=0.1, debug=False):
self._serial_port.dtr = True
self._serial_port.open()
self.debug = debug
+ self.lock = threading.Lock()
time.sleep(1)
def _respone(self):
@@ -55,11 +57,9 @@ def _respone(self):
qsize = self._get_queue_size()
print('respone_size2:', qsize)
break
-
+
except Exception as e:
print(e)
-
-
def _request(self, flag=""):
"""Get data from the robot"""
@@ -76,12 +76,12 @@ def _request(self, flag=""):
flag = None
if flag == "angle":
- a = data[data.find("ANGLES") :]
+ a = data[data.find("ANGLES"):]
astart = a.find("[")
aend = a.find("]")
if a != None and a != "":
try:
- all = list(map(float, a[astart + 1 : aend].split(",")))
+ all = list(map(float, a[astart + 1: aend].split(",")))
return all
except Exception:
print("received angles is not completed! Retry receive...")
@@ -89,12 +89,12 @@ def _request(self, flag=""):
continue
elif flag == "coord":
- c = data[data.find("COORDS") :]
+ c = data[data.find("COORDS"):]
cstart = c.find("[")
cend = c.find("]")
if c != None and c != "":
try:
- all = list(map(float, c[cstart + 1 : cend].split(",")))
+ all = list(map(float, c[cstart + 1: cend].split(",")))
return all
except Exception:
print("received coords is not completed! Retry receive...")
@@ -102,7 +102,7 @@ def _request(self, flag=""):
continue
elif flag == "endstop":
- e = data[data.find("ENDSTOP") :]
+ e = data[data.find("ENDSTOP"):]
eend = e.find("\n")
if e != None and e != "":
try:
@@ -115,11 +115,11 @@ def _request(self, flag=""):
elif flag == "size":
try:
- q = data[data.find("QUEUE SIZE") :]
+ q = data[data.find("QUEUE SIZE"):]
qstart = q.find("[")
qend = q.find("]")
if q != None and q != "":
- qsize = int(q[qstart + 1 : qend])
+ qsize = int(q[qstart + 1: qend])
return qsize
except Exception as e:
count += 1
@@ -131,15 +131,33 @@ def _request(self, flag=""):
return 0
elif flag == 'gripper':
header = "GRIPPERANGLE["
- read = data.find(header)+len(header)
+ read = data.find(header) + len(header)
# print(data[read:])
end = data[read:].find(']')
- return data[read:read+end]
+ return data[read:read + end]
elif flag == 'system':
header = "ReadSYS["
- read = data.find(header)+len(header)
+ read = data.find(header) + len(header)
end = data[read:].find(']')
- return data[read:read+end]
+ return int(data[read:read + end])
+ elif flag == 'system_version':
+ start_idx = data.find("GetSystemVersion[")
+ if start_idx != -1:
+ version_str = data[start_idx + len("GetSystemVersion["):]
+ version_str = version_str.split(']')[0]
+ version = float(version_str.strip())
+ return version
+ else:
+ return None
+ elif flag == 'modify_version':
+ start_idx = data.find("GetModifyVersion[")
+ if start_idx != -1:
+ version_str = data[start_idx + len("GetModifyVersion["):]
+ version_str = version_str.split(']')[0]
+ version = int(version_str.strip())
+ return version
+ else:
+ return None
elif flag == None:
return 0
@@ -158,150 +176,160 @@ def _get_queue_size(self):
def release_all_servos(self):
"""relax all joints."""
- command = ProtocolCode.RELEASE_SERVOS + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.RELEASE_SERVOS + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def power_on(self):
"""Lock all joints."""
- command = ProtocolCode.LOCK_SERVOS + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.LOCK_SERVOS + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def go_zero(self):
"""back to zero."""
- command = ProtocolCode.BACK_ZERO + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- time.sleep(0.1)
- data = None
- while True:
- if self._serial_port.inWaiting() > 0:
- data = self._serial_port.read(self._serial_port.inWaiting())
- if data != None:
- data = str(data.decode())
- if data.find("ok") > 0:
- if self.debug:
- print(data)
- break
+ with self.lock:
+ command = ProtocolCode.BACK_ZERO + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ time.sleep(0.1)
+ data = None
+ while True:
+ if self._serial_port.inWaiting() > 0:
+ data = self._serial_port.read(self._serial_port.inWaiting())
+ if data != None:
+ data = str(data.decode())
+ if data.find("ok") > 0:
+ if self.debug:
+ print(data)
+ break
def sleep(self, time):
- command = ProtocolCode.SLEEP_TIME + " S" + str(time) + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SLEEP_TIME + " S" + str(time) + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def get_angles_info(self):
"""Get the current joint angle information."""
- command = ProtocolCode.GET_CURRENT_ANGLE_INFO + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- return self._request("angle")
+ with self.lock:
+ command = ProtocolCode.GET_CURRENT_ANGLE_INFO + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("angle")
def get_radians_info(self):
"""Get the current joint radian information."""
- while True:
- angles = self.get_angles_info()
- if angles != None:
- break
- radians = []
- for i in angles:
- radians.append(round(i * (math.pi / 180), 3))
- return radians
+ with self.lock:
+ while True:
+ angles = self.get_angles_info()
+ if angles != None:
+ break
+ radians = []
+ for i in angles:
+ radians.append(round(i * (math.pi / 180), 3))
+ return radians
def get_coords_info(self):
"""Get current Cartesian coordinate information."""
- command = ProtocolCode.GET_CURRENT_COORD_INFO + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- return self._request("coord")
+ with self.lock:
+ command = ProtocolCode.GET_CURRENT_COORD_INFO + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("coord")
def get_switch_state(self):
"""Get the current state of all home switches."""
- command = ProtocolCode.GET_BACK_ZERO_STATUS + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- return self._request("endstop")
+ with self.lock:
+ command = ProtocolCode.GET_BACK_ZERO_STATUS + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("endstop")
def set_init_pose(self, x=None, y=None, z=None):
"""Set the current coords to zero."""
- command = ProtocolCode.SET_JOINT
- if x is not None:
- command += " x" + str(x)
- if y is not None:
- command += " y" + str(y)
- if z is not None:
- command += " z" + str(z)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SET_JOINT
+ if x is not None:
+ command += " x" + str(x)
+ if y is not None:
+ command += " y" + str(y)
+ if z is not None:
+ command += " z" + str(z)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_coords(self, degrees, speed=0):
"""Set all joints coords.
Args:
degrees: a list of coords value(List[float]).
- x : -260 ~ 300 mm
- y : -300 ~ 300 mm
- z : -70 ~ 135 mm
+ x : -360 ~ 365.55 mm
+ y : -365.55 ~ 365.55 mm
+ z : -140 ~ 130 mm
speed : (int) 0-200 mm/s
"""
- length = len(degrees)
- degrees = [degree for degree in degrees]
- command = ProtocolCode.COORDS_SET
- if degrees[0] is not None:
- command += " X" + str(degrees[0])
- if degrees[1] is not None:
- command += " Y" + str(degrees[1])
- if degrees[2] is not None:
- command += " Z" + str(degrees[2])
- if length == 4:
- if degrees[3] is not None:
- command += " E" + str(degrees[3])
- if speed > 0:
- command += " F" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ length = len(degrees)
+ degrees = [degree for degree in degrees]
+ command = ProtocolCode.COORDS_SET
+ if degrees[0] is not None:
+ command += " X" + str(degrees[0])
+ if degrees[1] is not None:
+ command += " Y" + str(degrees[1])
+ if degrees[2] is not None:
+ command += " Z" + str(degrees[2])
+ if length == 4:
+ if degrees[3] is not None:
+ command += " E" + str(degrees[3])
+ if speed > 0:
+ command += " F" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_coord(self, id=None, coord=None, speed=0):
"""Set single coord.
Args:
coord:
- x : -260 ~ 300 mm
- y : -300 ~ 300 mm
- z : -70 ~ 135 mm
+ x : -360 ~ 365.55 mm
+ y : -365.55 ~ 365.55 mm
+ z : -140 ~ 130 mm
speed : (int) 0-200 mm/s
"""
-
- command = ProtocolCode.COORDS_SET
- if id == "x" or id == "X":
- command += " X" + str(coord)
- if id == "y" or id == "Y":
- command += " Y" + str(coord)
- if id == "z" or id == "Z":
- command += " Z" + str(coord)
- if speed > 0:
- command += " F" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.COORDS_SET
+ if id == "x" or id == "X":
+ command += " X" + str(coord)
+ if id == "y" or id == "Y":
+ command += " Y" + str(coord)
+ if id == "z" or id == "Z":
+ command += " Z" + str(coord)
+ if speed > 0:
+ command += " F" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_mode(self, mode=None):
"""set coord mode.
@@ -312,15 +340,16 @@ def set_mode(self, mode=None):
1 - Relative Cartesian mode
"""
- if mode == 0:
- command = ProtocolCode.ABS_CARTESIAN + ProtocolCode.END
- else:
- command = ProtocolCode.REL_CARTESIAN + ProtocolCode.END
+ with self.lock:
+ if mode == 0:
+ command = ProtocolCode.ABS_CARTESIAN + ProtocolCode.END
+ else:
+ command = ProtocolCode.REL_CARTESIAN + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_speed_mode(self, mode=None):
"""set speed mode.
@@ -331,14 +360,15 @@ def set_speed_mode(self, mode=None):
2 - Acceleration / deceleration mode
"""
- if mode != None:
- command = "M16 S" + str(mode) + ProtocolCode.END
- else:
- print("Please enter the correct value!")
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ if mode != None:
+ command = "M16 S" + str(mode) + ProtocolCode.END
+ else:
+ print("Please enter the correct value!")
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_pwm(self, p=None):
"""PWM control.
@@ -346,11 +376,12 @@ def set_pwm(self, p=None):
Args:
p (int) : Duty cycle 0 ~ 255; 128 means 50%
"""
- command = ProtocolCode.SET_PWM + "p" + str(p) + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SET_PWM + "p" + str(p) + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_gpio_state(self, state):
"""Set gpio state.
@@ -360,21 +391,23 @@ def set_gpio_state(self, state):
1 - open
0 - close
"""
- if state:
- command = ProtocolCode.GPIO_CLOSE + ProtocolCode.END
- else:
- command = ProtocolCode.GPIO_ON + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ if state:
+ command = ProtocolCode.GPIO_CLOSE + ProtocolCode.END
+ else:
+ command = ProtocolCode.GPIO_ON + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_gripper_zero(self):
- command = ProtocolCode.GRIPPER_ZERO + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.GRIPPER_ZERO + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_gripper_state(self, state, speed):
"""Set gripper state.
@@ -385,18 +418,20 @@ def set_gripper_state(self, state, speed):
100 - full open
speed: 0 - 1500
"""
- command = ProtocolCode.GIRPPER_OPEN + "A" + str(state) + "F" + str(speed) + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.GIRPPER_OPEN + "A" + str(state) + "F" + str(speed) + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_gripper_release(self):
- command = ProtocolCode.GIRPPER_RELEASE + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.GIRPPER_RELEASE + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_fan_state(self, state):
"""Set fan state.
@@ -406,14 +441,15 @@ def set_fan_state(self, state):
0 - close
1 - open
"""
- if state:
- command = ProtocolCode.FAN_ON + ProtocolCode.END
- else:
- command = ProtocolCode.FAN_CLOSE + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ if state:
+ command = ProtocolCode.FAN_ON + ProtocolCode.END
+ else:
+ command = ProtocolCode.FAN_CLOSE + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_angle(self, id=None, angle=None, speed=0):
"""Set single angle.
@@ -424,22 +460,23 @@ def set_angle(self, id=None, angle=None, speed=0):
angle :
1 : -150° ~ +170°
2 : -20° ~ 90°
- 3 : -5° ~ 60°
- 4 : -180° ~ + 180°
+ 3 : -5° ~ 110°
+ 4 : -179° ~ + 179°
speed : (int) 0-200 mm/s
"""
- command = ProtocolCode.SET_ANGLE
- if id is not None:
- command += " j" + str(id)
- if angle is not None:
- command += " a" + str(angle)
- if speed > 0:
- command += " f" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SET_ANGLE
+ if id is not None:
+ command += " j" + str(id)
+ if angle is not None:
+ command += " a" + str(angle)
+ if speed > 0:
+ command += " f" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_angles(self, degrees, speed=0):
"""Set all joints angles.
@@ -449,42 +486,46 @@ def set_angles(self, degrees, speed=0):
angle :
1 : -150° ~ +170°
2 : -20° ~ 90°
- 3 : -5° ~ 60°
- 4 : -180° ~ + 180°
+ 3 : -5° ~ 110°
+ 4 : -179° ~ + 179°
speed : (int) 0-200 mm/s
"""
- length = len(degrees)
- degrees = [degree for degree in degrees]
- command = ProtocolCode.SET_ANGLES
- if degrees[0] is not None:
- command += " X" + str(degrees[0])
- if degrees[1] is not None:
- command += " Y" + str(degrees[1])
- if degrees[2] is not None:
- command += " Z" + str(degrees[2])
- if length == 4:
- if degrees[3] is not None:
- command += " E" + str(degrees[3])
- if speed > 0:
- command += " F" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ length = len(degrees)
+ degrees = [degree for degree in degrees]
+ command = ProtocolCode.SET_ANGLES
+ if degrees[0] is not None:
+ command += " X" + str(degrees[0])
+ if degrees[1] is not None:
+ command += " Y" + str(degrees[1])
+ if degrees[2] is not None:
+ command += " Z" + str(degrees[2])
+ if length == 4:
+ if degrees[3] is not None:
+ command += " E" + str(degrees[3])
+ if speed > 0:
+ command += " F" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_radians(self, degrees, speed=0):
"""Set all joints radians
Args:
degrees: a list of radians value(List[float]).
- rx : -2.97 ~ +2.97
- ry : 0.0 ~ 1.57
- rz : 0.0 ~ 1.31
+ angle :
+ 1 : 2.6179 ~ 2.9670
+ 2 : -0.3490 ~ 1.5707
+ 3 : -0.0872 ~ 1.9198
+ 4 : -3.1241 ~ + 3.1241
speed : (int) 0-200 mm/s
"""
- degrees = [round(degree * (180 / math.pi), 2) for degree in degrees]
- self.set_angles(degrees, speed)
+ with self.lock:
+ degrees = [round(degree * (180 / math.pi), 2) for degree in degrees]
+ self.set_angles(degrees, speed)
def set_jog_angle(self, id=None, direction=None, speed=0):
"""Start jog movement with angle
@@ -497,18 +538,19 @@ def set_jog_angle(self, id=None, direction=None, speed=0):
1 : negative
speed : (int) 0-200 mm/s
"""
- command = ProtocolCode.SET_JOG_ANGLE
- if id is not None:
- command += " j" + str(id)
- if direction is not None:
- command += " d" + str(direction)
- if speed > 0:
- command += " f" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SET_JOG_ANGLE
+ if id is not None:
+ command += " j" + str(id)
+ if direction is not None:
+ command += " d" + str(direction)
+ if speed > 0:
+ command += " f" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_jog_coord(self, axis=None, direction=None, speed=0):
"""Start jog movement with coord
@@ -525,93 +567,99 @@ def set_jog_coord(self, axis=None, direction=None, speed=0):
1 : negative
speed : (int) 0-200 mm/s
"""
- command = ProtocolCode.JOG_COORD_
- if axis is not None:
- command += " j" + str(axis)
- if direction is not None:
- command += " d" + str(direction)
- if speed > 0:
- command += " f" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.JOG_COORD_
+ if axis is not None:
+ command += " j" + str(axis)
+ if direction is not None:
+ command += " d" + str(direction)
+ if speed > 0:
+ command += " f" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_jog_stop(self):
"""Stop jog movement"""
- command = ProtocolCode.SET_JOG_STOP
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
-
- def play_gcode_file(self, filename=None):
- """Play the imported track file"""
- X = []
- try:
- with open(filename) as f:
- lines = f.readlines()
- for line in lines:
- line = line.strip("\n")
- X.append(line)
- except Exception:
- print("There is no such file!")
-
- begin, end = 0, len(X)
-
- self.set_speed_mode(0) # Constant speed mode
-
- for i in range(begin, end):
- command = X[i] + ProtocolCode.END
+ with self.lock:
+ command = ProtocolCode.SET_JOG_STOP
+ command += ProtocolCode.END
self._serial_port.write(command.encode())
self._serial_port.flush()
- time.sleep(0.02)
-
self._debug(command)
+ self._respone()
- queue_size = self._request("size")
- if 0 <= queue_size <= 90:
- try:
- if self.debug:
- print("queue_size1: %s \n" % queue_size)
- except Exception as e:
- print(e)
- else:
- qs = 0
- while True:
+ def play_gcode_file(self, filename=None):
+ """Play the imported track file"""
+ with self.lock:
+ X = []
+ try:
+ with open(filename) as f:
+ lines = f.readlines()
+ for line in lines:
+ line = line.strip("\n")
+ X.append(line)
+ except Exception:
+ print("There is no such file!")
+
+ begin, end = 0, len(X)
+
+ self.set_speed_mode(0) # Constant speed mode
+
+ for i in range(begin, end):
+ command = X[i] + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ time.sleep(0.02)
+
+ self._debug(command)
+
+ queue_size = self._request("size")
+ if 0 <= queue_size <= 90:
try:
- qs = self._get_queue_size()
if self.debug:
- print("queue_size2: %s \n" % qs)
- if qs <= 40:
- begin = i
- break
- except Exception:
- print("Retry receive...")
- command = ""
+ print("queue_size1: %s \n" % queue_size)
+ except Exception as e:
+ print(e)
+ else:
+ qs = 0
+ while True:
+ try:
+ qs = self._get_queue_size()
+ if self.debug:
+ print("queue_size2: %s \n" % qs)
+ if qs <= 40:
+ begin = i
+ break
+ except Exception:
+ print("Retry receive...")
+ command = ""
self.set_speed_mode(2) # Acceleration / deceleration mode
-
+
def close(self):
- self._serial_port.close()
+ with self.lock:
+ self._serial_port.close()
def open(self):
- self._serial_port.open()
+ with self.lock:
+ self._serial_port.open()
def is_moving_end(self):
- """Get the current state of all home switches."""
- command = ProtocolCode.IS_MOVING_END + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- return self._request("isStop")
+ with self.lock:
+ """Get the current state of all home switches."""
+ command = ProtocolCode.IS_MOVING_END + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("isStop")
def sync(self):
while self.is_moving_end() != 1:
pass
-
+
def get_gripper_angle(self):
"""
@@ -621,32 +669,73 @@ def get_gripper_angle(self):
self._serial_port.flush()
self._debug(command)
return self._request("gripper")
-
- def set_system_value(self, id, address, value):
+
+ def set_system_value(self, id, address, value, mode=None):
"""_summary_
Args:
id (int): 4 or 7
- address (int): 0 ~ 69
- value (int):
+ address (int): 7 ~ 69
+ value (int):
+ mode (int): 1 or 2, can be empty, default mode is 1
+ 1 - setting range is 0-255, address 21 (P value) can be used
+ 2 - setting value range is 0-65535, address 56 (setting position) can be used
"""
- command = ProtocolCode.SET_SYSTEM_VALUE +" X{} ".format(id) + "Y{} ".format(address) +"Z{} ".format(value)+ ProtocolCode.END
+ if mode:
+ command = ProtocolCode.SET_SYSTEM_VALUE + " X{} ".format(id) + "Y{} ".format(address) + "Z{} ".format(
+ value) + "P{} ".format(mode) + ProtocolCode.END
+ else:
+ command = ProtocolCode.SET_SYSTEM_VALUE + " X{} ".format(id) + "Y{} ".format(address) + "Z{} ".format(
+ value) + ProtocolCode.END
self._serial_port.write(command.encode())
self._serial_port.flush()
self._debug(command)
-
- def get_system_value(self, id, address):
+
+ def get_system_value(self, id, address, mode=None):
"""_summary_
Args:
id (int): 4 or 7
address (_type_): 0 ~ 69
+ mode (int): 1 or 2, can be empty, default mode is 1
+ 1 - read range is 0-255, address 21 (P value) can be used
+ 2 - read value range is 0-65535, address 56 (read position) can be used
Returns:
_type_: _description_
"""
- command = ProtocolCode.GET_SYSTEM_VALUE + " J{} ".format(id) + "S{} ".format(address) +ProtocolCode.END
+ if mode:
+ command = ProtocolCode.GET_SYSTEM_VALUE + " J{} ".format(id) + "S{} ".format(address) + "P{} ".format(
+ mode) + ProtocolCode.END
+ else:
+ command = ProtocolCode.GET_SYSTEM_VALUE + " J{} ".format(id) + "S{} ".format(address) + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("system")
+
+ def get_system_version(self):
+ """
+ Get system firmware version
+
+ Returns:
+ (float) Firmware version
+ """
+ command = ProtocolCode.GET_SYSTEM_VERSION + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request('system_version')
+
+ def get_modify_version(self):
+ """
+ Get firmware modify version
+
+ Returns:
+ (int) modify version
+ """
+ command = ProtocolCode.GET_MODIFY_VERSION + ProtocolCode.END
self._serial_port.write(command.encode())
self._serial_port.flush()
self._debug(command)
- return self._request("system")
\ No newline at end of file
+ return self._request('modify_version')
diff --git a/pymycobot/ultraArmP340.py b/pymycobot/ultraArmP340.py
index a91d56a..22bcb95 100644
--- a/pymycobot/ultraArmP340.py
+++ b/pymycobot/ultraArmP340.py
@@ -2,6 +2,7 @@
import time
from pymycobot.common import ProtocolCode
import math
+import threading
class ultraArmP340:
@@ -24,6 +25,7 @@ def __init__(self, port, baudrate=115200, timeout=0.1, debug=False):
self._serial_port.dtr = True
self._serial_port.open()
self.debug = debug
+ self.lock = threading.Lock()
time.sleep(1)
def _respone(self):
@@ -137,7 +139,25 @@ def _request(self, flag=""):
header = "ReadSYS["
read = data.find(header) + len(header)
end = data[read:].find(']')
- return data[read:read + end]
+ return int(data[read:read + end])
+ elif flag == 'system_version':
+ start_idx = data.find("GetSystemVersion[")
+ if start_idx != -1:
+ version_str = data[start_idx + len("GetSystemVersion["):]
+ version_str = version_str.split(']')[0]
+ version = float(version_str.strip())
+ return version
+ else:
+ return None
+ elif flag == 'modify_version':
+ start_idx = data.find("GetModifyVersion[")
+ if start_idx != -1:
+ version_str = data[start_idx + len("GetModifyVersion["):]
+ version_str = version_str.split(']')[0]
+ version = int(version_str.strip())
+ return version
+ else:
+ return None
elif flag == None:
return 0
@@ -156,150 +176,160 @@ def _get_queue_size(self):
def release_all_servos(self):
"""relax all joints."""
- command = ProtocolCode.RELEASE_SERVOS + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.RELEASE_SERVOS + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def power_on(self):
"""Lock all joints."""
- command = ProtocolCode.LOCK_SERVOS + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.LOCK_SERVOS + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def go_zero(self):
"""back to zero."""
- command = ProtocolCode.BACK_ZERO + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- time.sleep(0.1)
- data = None
- while True:
- if self._serial_port.inWaiting() > 0:
- data = self._serial_port.read(self._serial_port.inWaiting())
- if data != None:
- data = str(data.decode())
- if data.find("ok") > 0:
- if self.debug:
- print(data)
- break
+ with self.lock:
+ command = ProtocolCode.BACK_ZERO + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ time.sleep(0.1)
+ data = None
+ while True:
+ if self._serial_port.inWaiting() > 0:
+ data = self._serial_port.read(self._serial_port.inWaiting())
+ if data != None:
+ data = str(data.decode())
+ if data.find("ok") > 0:
+ if self.debug:
+ print(data)
+ break
def sleep(self, time):
- command = ProtocolCode.SLEEP_TIME + " S" + str(time) + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SLEEP_TIME + " S" + str(time) + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def get_angles_info(self):
"""Get the current joint angle information."""
- command = ProtocolCode.GET_CURRENT_ANGLE_INFO + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- return self._request("angle")
+ with self.lock:
+ command = ProtocolCode.GET_CURRENT_ANGLE_INFO + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("angle")
def get_radians_info(self):
"""Get the current joint radian information."""
- while True:
- angles = self.get_angles_info()
- if angles != None:
- break
- radians = []
- for i in angles:
- radians.append(round(i * (math.pi / 180), 3))
- return radians
+ with self.lock:
+ while True:
+ angles = self.get_angles_info()
+ if angles != None:
+ break
+ radians = []
+ for i in angles:
+ radians.append(round(i * (math.pi / 180), 3))
+ return radians
def get_coords_info(self):
"""Get current Cartesian coordinate information."""
- command = ProtocolCode.GET_CURRENT_COORD_INFO + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- return self._request("coord")
+ with self.lock:
+ command = ProtocolCode.GET_CURRENT_COORD_INFO + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("coord")
def get_switch_state(self):
"""Get the current state of all home switches."""
- command = ProtocolCode.GET_BACK_ZERO_STATUS + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- return self._request("endstop")
+ with self.lock:
+ command = ProtocolCode.GET_BACK_ZERO_STATUS + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("endstop")
def set_init_pose(self, x=None, y=None, z=None):
"""Set the current coords to zero."""
- command = ProtocolCode.SET_JOINT
- if x is not None:
- command += " x" + str(x)
- if y is not None:
- command += " y" + str(y)
- if z is not None:
- command += " z" + str(z)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SET_JOINT
+ if x is not None:
+ command += " x" + str(x)
+ if y is not None:
+ command += " y" + str(y)
+ if z is not None:
+ command += " z" + str(z)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_coords(self, degrees, speed=0):
"""Set all joints coords.
Args:
degrees: a list of coords value(List[float]).
- x : -260 ~ 300 mm
- y : -300 ~ 300 mm
- z : -70 ~ 135 mm
+ x : -360 ~ 365.55 mm
+ y : -365.55 ~ 365.55 mm
+ z : -140 ~ 130 mm
speed : (int) 0-200 mm/s
"""
- length = len(degrees)
- degrees = [degree for degree in degrees]
- command = ProtocolCode.COORDS_SET
- if degrees[0] is not None:
- command += " X" + str(degrees[0])
- if degrees[1] is not None:
- command += " Y" + str(degrees[1])
- if degrees[2] is not None:
- command += " Z" + str(degrees[2])
- if length == 4:
- if degrees[3] is not None:
- command += " E" + str(degrees[3])
- if speed > 0:
- command += " F" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ length = len(degrees)
+ degrees = [degree for degree in degrees]
+ command = ProtocolCode.COORDS_SET
+ if degrees[0] is not None:
+ command += " X" + str(degrees[0])
+ if degrees[1] is not None:
+ command += " Y" + str(degrees[1])
+ if degrees[2] is not None:
+ command += " Z" + str(degrees[2])
+ if length == 4:
+ if degrees[3] is not None:
+ command += " E" + str(degrees[3])
+ if speed > 0:
+ command += " F" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_coord(self, id=None, coord=None, speed=0):
"""Set single coord.
Args:
coord:
- x : -260 ~ 300 mm
- y : -300 ~ 300 mm
- z : -70 ~ 135 mm
+ x : -360 ~ 365.55 mm
+ y : -365.55 ~ 365.55 mm
+ z : -140 ~ 130 mm
speed : (int) 0-200 mm/s
"""
-
- command = ProtocolCode.COORDS_SET
- if id == "x" or id == "X":
- command += " X" + str(coord)
- if id == "y" or id == "Y":
- command += " Y" + str(coord)
- if id == "z" or id == "Z":
- command += " Z" + str(coord)
- if speed > 0:
- command += " F" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.COORDS_SET
+ if id == "x" or id == "X":
+ command += " X" + str(coord)
+ if id == "y" or id == "Y":
+ command += " Y" + str(coord)
+ if id == "z" or id == "Z":
+ command += " Z" + str(coord)
+ if speed > 0:
+ command += " F" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_mode(self, mode=None):
"""set coord mode.
@@ -310,15 +340,16 @@ def set_mode(self, mode=None):
1 - Relative Cartesian mode
"""
- if mode == 0:
- command = ProtocolCode.ABS_CARTESIAN + ProtocolCode.END
- else:
- command = ProtocolCode.REL_CARTESIAN + ProtocolCode.END
+ with self.lock:
+ if mode == 0:
+ command = ProtocolCode.ABS_CARTESIAN + ProtocolCode.END
+ else:
+ command = ProtocolCode.REL_CARTESIAN + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_speed_mode(self, mode=None):
"""set speed mode.
@@ -329,14 +360,15 @@ def set_speed_mode(self, mode=None):
2 - Acceleration / deceleration mode
"""
- if mode != None:
- command = "M16 S" + str(mode) + ProtocolCode.END
- else:
- print("Please enter the correct value!")
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ if mode != None:
+ command = "M16 S" + str(mode) + ProtocolCode.END
+ else:
+ print("Please enter the correct value!")
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_pwm(self, p=None):
"""PWM control.
@@ -344,11 +376,12 @@ def set_pwm(self, p=None):
Args:
p (int) : Duty cycle 0 ~ 255; 128 means 50%
"""
- command = ProtocolCode.SET_PWM + "p" + str(p) + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SET_PWM + "p" + str(p) + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_gpio_state(self, state):
"""Set gpio state.
@@ -358,21 +391,23 @@ def set_gpio_state(self, state):
1 - open
0 - close
"""
- if state:
- command = ProtocolCode.GPIO_CLOSE + ProtocolCode.END
- else:
- command = ProtocolCode.GPIO_ON + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ if state:
+ command = ProtocolCode.GPIO_CLOSE + ProtocolCode.END
+ else:
+ command = ProtocolCode.GPIO_ON + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_gripper_zero(self):
- command = ProtocolCode.GRIPPER_ZERO + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.GRIPPER_ZERO + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_gripper_state(self, state, speed):
"""Set gripper state.
@@ -383,18 +418,20 @@ def set_gripper_state(self, state, speed):
100 - full open
speed: 0 - 1500
"""
- command = ProtocolCode.GIRPPER_OPEN + "A" + str(state) + "F" + str(speed) + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.GIRPPER_OPEN + "A" + str(state) + "F" + str(speed) + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_gripper_release(self):
- command = ProtocolCode.GIRPPER_RELEASE + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.GIRPPER_RELEASE + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_fan_state(self, state):
"""Set fan state.
@@ -404,14 +441,15 @@ def set_fan_state(self, state):
0 - close
1 - open
"""
- if state:
- command = ProtocolCode.FAN_ON + ProtocolCode.END
- else:
- command = ProtocolCode.FAN_CLOSE + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ if state:
+ command = ProtocolCode.FAN_ON + ProtocolCode.END
+ else:
+ command = ProtocolCode.FAN_CLOSE + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_angle(self, id=None, angle=None, speed=0):
"""Set single angle.
@@ -422,22 +460,23 @@ def set_angle(self, id=None, angle=None, speed=0):
angle :
1 : -150° ~ +170°
2 : -20° ~ 90°
- 3 : -5° ~ 60°
- 4 : -180° ~ + 180°
+ 3 : -5° ~ 110°
+ 4 : -179° ~ + 179°
speed : (int) 0-200 mm/s
"""
- command = ProtocolCode.SET_ANGLE
- if id is not None:
- command += " j" + str(id)
- if angle is not None:
- command += " a" + str(angle)
- if speed > 0:
- command += " f" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SET_ANGLE
+ if id is not None:
+ command += " j" + str(id)
+ if angle is not None:
+ command += " a" + str(angle)
+ if speed > 0:
+ command += " f" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_angles(self, degrees, speed=0):
"""Set all joints angles.
@@ -447,42 +486,46 @@ def set_angles(self, degrees, speed=0):
angle :
1 : -150° ~ +170°
2 : -20° ~ 90°
- 3 : -5° ~ 60°
- 4 : -180° ~ + 180°
+ 3 : -5° ~ 110°
+ 4 : -179° ~ + 179°
speed : (int) 0-200 mm/s
"""
- length = len(degrees)
- degrees = [degree for degree in degrees]
- command = ProtocolCode.SET_ANGLES
- if degrees[0] is not None:
- command += " X" + str(degrees[0])
- if degrees[1] is not None:
- command += " Y" + str(degrees[1])
- if degrees[2] is not None:
- command += " Z" + str(degrees[2])
- if length == 4:
- if degrees[3] is not None:
- command += " E" + str(degrees[3])
- if speed > 0:
- command += " F" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ length = len(degrees)
+ degrees = [degree for degree in degrees]
+ command = ProtocolCode.SET_ANGLES
+ if degrees[0] is not None:
+ command += " X" + str(degrees[0])
+ if degrees[1] is not None:
+ command += " Y" + str(degrees[1])
+ if degrees[2] is not None:
+ command += " Z" + str(degrees[2])
+ if length == 4:
+ if degrees[3] is not None:
+ command += " E" + str(degrees[3])
+ if speed > 0:
+ command += " F" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_radians(self, degrees, speed=0):
"""Set all joints radians
Args:
degrees: a list of radians value(List[float]).
- rx : -2.97 ~ +2.97
- ry : 0.0 ~ 1.57
- rz : 0.0 ~ 1.31
+ angle :
+ 1 : 2.6179 ~ 2.9670
+ 2 : -0.3490 ~ 1.5707
+ 3 : -0.0872 ~ 1.9198
+ 4 : -3.1241 ~ + 3.1241
speed : (int) 0-200 mm/s
"""
- degrees = [round(degree * (180 / math.pi), 2) for degree in degrees]
- self.set_angles(degrees, speed)
+ with self.lock:
+ degrees = [round(degree * (180 / math.pi), 2) for degree in degrees]
+ self.set_angles(degrees, speed)
def set_jog_angle(self, id=None, direction=None, speed=0):
"""Start jog movement with angle
@@ -495,18 +538,19 @@ def set_jog_angle(self, id=None, direction=None, speed=0):
1 : negative
speed : (int) 0-200 mm/s
"""
- command = ProtocolCode.SET_JOG_ANGLE
- if id is not None:
- command += " j" + str(id)
- if direction is not None:
- command += " d" + str(direction)
- if speed > 0:
- command += " f" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.SET_JOG_ANGLE
+ if id is not None:
+ command += " j" + str(id)
+ if direction is not None:
+ command += " d" + str(direction)
+ if speed > 0:
+ command += " f" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_jog_coord(self, axis=None, direction=None, speed=0):
"""Start jog movement with coord
@@ -523,88 +567,94 @@ def set_jog_coord(self, axis=None, direction=None, speed=0):
1 : negative
speed : (int) 0-200 mm/s
"""
- command = ProtocolCode.JOG_COORD_
- if axis is not None:
- command += " j" + str(axis)
- if direction is not None:
- command += " d" + str(direction)
- if speed > 0:
- command += " f" + str(speed)
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
+ with self.lock:
+ command = ProtocolCode.JOG_COORD_
+ if axis is not None:
+ command += " j" + str(axis)
+ if direction is not None:
+ command += " d" + str(direction)
+ if speed > 0:
+ command += " f" + str(speed)
+ command += ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ self._respone()
def set_jog_stop(self):
"""Stop jog movement"""
- command = ProtocolCode.SET_JOG_STOP
- command += ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- self._respone()
-
- def play_gcode_file(self, filename=None):
- """Play the imported track file"""
- X = []
- try:
- with open(filename) as f:
- lines = f.readlines()
- for line in lines:
- line = line.strip("\n")
- X.append(line)
- except Exception:
- print("There is no such file!")
-
- begin, end = 0, len(X)
-
- self.set_speed_mode(0) # Constant speed mode
-
- for i in range(begin, end):
- command = X[i] + ProtocolCode.END
+ with self.lock:
+ command = ProtocolCode.SET_JOG_STOP
+ command += ProtocolCode.END
self._serial_port.write(command.encode())
self._serial_port.flush()
- time.sleep(0.02)
-
self._debug(command)
+ self._respone()
- queue_size = self._request("size")
- if 0 <= queue_size <= 90:
- try:
- if self.debug:
- print("queue_size1: %s \n" % queue_size)
- except Exception as e:
- print(e)
- else:
- qs = 0
- while True:
+ def play_gcode_file(self, filename=None):
+ """Play the imported track file"""
+ with self.lock:
+ X = []
+ try:
+ with open(filename) as f:
+ lines = f.readlines()
+ for line in lines:
+ line = line.strip("\n")
+ X.append(line)
+ except Exception:
+ print("There is no such file!")
+
+ begin, end = 0, len(X)
+
+ self.set_speed_mode(0) # Constant speed mode
+
+ for i in range(begin, end):
+ command = X[i] + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ time.sleep(0.02)
+
+ self._debug(command)
+
+ queue_size = self._request("size")
+ if 0 <= queue_size <= 90:
try:
- qs = self._get_queue_size()
if self.debug:
- print("queue_size2: %s \n" % qs)
- if qs <= 40:
- begin = i
- break
- except Exception:
- print("Retry receive...")
- command = ""
+ print("queue_size1: %s \n" % queue_size)
+ except Exception as e:
+ print(e)
+ else:
+ qs = 0
+ while True:
+ try:
+ qs = self._get_queue_size()
+ if self.debug:
+ print("queue_size2: %s \n" % qs)
+ if qs <= 40:
+ begin = i
+ break
+ except Exception:
+ print("Retry receive...")
+ command = ""
self.set_speed_mode(2) # Acceleration / deceleration mode
def close(self):
- self._serial_port.close()
-
+ with self.lock:
+ self._serial_port.close()
+
def open(self):
- self._serial_port.open()
-
+ with self.lock:
+ self._serial_port.open()
+
def is_moving_end(self):
- """Get the current state of all home switches."""
- command = ProtocolCode.IS_MOVING_END + ProtocolCode.END
- self._serial_port.write(command.encode())
- self._serial_port.flush()
- self._debug(command)
- return self._request("isStop")
+ with self.lock:
+ """Get the current state of all home switches."""
+ command = ProtocolCode.IS_MOVING_END + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request("isStop")
def sync(self):
while self.is_moving_end() != 1:
@@ -620,33 +670,72 @@ def get_gripper_angle(self):
self._debug(command)
return self._request("gripper")
- def set_system_value(self, id, address, value):
+ def set_system_value(self, id, address, value, mode=None):
"""_summary_
Args:
id (int): 4 or 7
- address (int): 0 ~ 69
- value (int):
+ address (int): 7 ~ 69
+ value (int):
+ mode (int): 1 or 2, can be empty, default mode is 1
+ 1 - setting range is 0-255, address 21 (P value) can be used
+ 2 - setting value range is 0-65535, address 56 (setting position) can be used
"""
- command = ProtocolCode.SET_SYSTEM_VALUE + " X{} ".format(id) + "Y{} ".format(address) + "Z{} ".format(
- value) + ProtocolCode.END
+ if mode:
+ command = ProtocolCode.SET_SYSTEM_VALUE + " X{} ".format(id) + "Y{} ".format(address) + "Z{} ".format(
+ value) + "P{} ".format(mode) + ProtocolCode.END
+ else:
+ command = ProtocolCode.SET_SYSTEM_VALUE + " X{} ".format(id) + "Y{} ".format(address) + "Z{} ".format(
+ value) + ProtocolCode.END
self._serial_port.write(command.encode())
self._serial_port.flush()
self._debug(command)
- def get_system_value(self, id, address):
+ def get_system_value(self, id, address, mode=None):
"""_summary_
Args:
id (int): 4 or 7
address (_type_): 0 ~ 69
+ mode (int): 1 or 2, can be empty, default mode is 1
+ 1 - read range is 0-255, address 21 (P value) can be used
+ 2 - read value range is 0-65535, address 56 (read position) can be used
Returns:
_type_: _description_
"""
- command = ProtocolCode.GET_SYSTEM_VALUE + " J{} ".format(id) + "S{} ".format(address) + ProtocolCode.END
+ if mode:
+ command = ProtocolCode.GET_SYSTEM_VALUE + " J{} ".format(id) + "S{} ".format(address) + "P{} ".format(
+ mode) + ProtocolCode.END
+ else:
+ command = ProtocolCode.GET_SYSTEM_VALUE + " J{} ".format(id) + "S{} ".format(address) + ProtocolCode.END
self._serial_port.write(command.encode())
self._serial_port.flush()
self._debug(command)
return self._request("system")
+ def get_system_version(self):
+ """
+ Get system firmware version
+
+ Returns:
+ (float) Firmware version
+ """
+ command = ProtocolCode.GET_SYSTEM_VERSION + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request('system_version')
+
+ def get_modify_version(self):
+ """
+ Get firmware modify version
+
+ Returns:
+ (int) modify version
+ """
+ command = ProtocolCode.GET_MODIFY_VERSION + ProtocolCode.END
+ self._serial_port.write(command.encode())
+ self._serial_port.flush()
+ self._debug(command)
+ return self._request('modify_version')
diff --git a/requirements.txt b/requirements.txt
index bce098f..5cbeabd 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -3,4 +3,5 @@ python-can
pytest
flake8
opencv-python
+numpy
crc
diff --git a/setup.py b/setup.py
index 4b4ddf5..2588d3c 100644
--- a/setup.py
+++ b/setup.py
@@ -88,6 +88,6 @@
"License :: OSI Approved :: MIT License",
"Operating System :: OS Independent",
],
- install_requires=["pyserial", "python-can", "crc"],
+ install_requires=["pyserial","python-can", "numpy", "python-can", "crc"],
python_requires=">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*, != 3.4.*",
)
diff --git a/tests/__init__.py b/tests/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/tests/conftest.py b/tests/conftest.py
new file mode 100644
index 0000000..e69de29
diff --git a/tests/test_api.py b/tests/test_api.py
index 68336da..5cef1e2 100644
--- a/tests/test_api.py
+++ b/tests/test_api.py
@@ -17,7 +17,7 @@
@pytest.fixture(scope="module")
-def setup():
+def setup():
print("")
global port, mc