Skip to content

Commit 0ba5648

Browse files
anla-xuanla-xu
committed
Added some interfaces
1 parent b9ea148 commit 0ba5648

File tree

6 files changed

+166
-66
lines changed

6 files changed

+166
-66
lines changed

‎pymycobot/Interface.py‎

Lines changed: 74 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -25,9 +25,9 @@ def _mesg(self, genre, *args, **kwargs):
2525
command_data = self._encode_int16(command_data)
2626

2727
LEN = len(command_data) + 1
28-
check_digit = sum(command_data[1:]) + genre
29-
if check_digit >= 256:
30-
check_digit %= 256
28+
check_digit = (sum(command_data[1:]) + genre) & 0xff
29+
# if check_digit >= 256:
30+
# check_digit %= 256
3131
command = [
3232
ProtocolCode.HEADER,
3333
ProtocolCode.HEADER,
@@ -843,33 +843,6 @@ def get_joint_current(self, id, joint_id):
843843
"""
844844
return self._mesg(ProtocolCode.GET_JOINT_CURRENT, id, joint_id)
845845

846-
# Basic
847-
def set_basic_mode(self, pin_no, pin_mode):
848-
"""Set base IO, input or output mode
849-
850-
Args:
851-
pin_no: 1 - 5
852-
pin_mode: 0 - input 1 - output
853-
"""
854-
return self._mesg(ProtocolCode.SET_BASIC_OUTPUT, 0, pin_no, pin_mode)
855-
856-
def set_basic_output(self, pin_no, pin_signal):
857-
"""Set basic output.
858-
859-
Args:
860-
pin_no: pin port number (0 - 20).
861-
pin_signal: 0 / 1 (0 - low, 1 - high)
862-
"""
863-
return self._mesg(ProtocolCode.GET_BASIC_INPUT, 0, pin_no, pin_signal)
864-
865-
def get_basic_input(self, pin_no):
866-
"""Get basic input for M5 version.
867-
868-
Args:
869-
pin_no: (int) pin port number (0 - 20).
870-
"""
871-
return self._mesg(ProtocolCode.GET_BASE_INPUT, 0, pin_no, has_reply=True)
872-
873846
def get_plan_speed(self, id = 0):
874847
"""Get planning speed
875848
@@ -953,8 +926,8 @@ def get_servo_temps(self, id):
953926
"""
954927
return self._mesg(0xE6, id, has_reply=True)
955928

956-
def get_base_coords(self, coords, arm):
957-
"""Convert coordinates to base coordinates
929+
def get_base_coords(self, *args: int):
930+
"""Convert coordinates to base coordinates. Pass in parameters or no parameters
958931
959932
Args:
960933
coords: a list of coords value(List[float]), length 6 [x(mm), y, z, rx(angle), ry, rz]
@@ -963,12 +936,16 @@ def get_base_coords(self, coords, arm):
963936
Return:
964937
Base coords
965938
"""
966-
coord_list = []
967-
for idx in range(3):
968-
coord_list.append(self._coord2int(coords[idx]))
969-
for angle in coords[3:]:
970-
coord_list.append(self._angle2int(angle))
971-
return self._mesg(ProtocolCode.GET_BASE_COORDS, 0, coord_list, arm, has_reply = True)
939+
if len(args) == 2:
940+
coords, arm = args
941+
coord_list = []
942+
for idx in range(3):
943+
coord_list.append(self._coord2int(coords[idx]))
944+
for angle in coords[3:]:
945+
coord_list.append(self._angle2int(angle))
946+
return self._mesg(ProtocolCode.GET_BASE_COORDS, 0, coord_list, arm, has_reply = True)
947+
elif len(args) == 0:
948+
return self._mesg(ProtocolCode.GET_ALL_BASE_COORDS, 0, has_reply = True)
972949

973950
def base_to_single_coords(self, base_coords, arm):
974951
"""Convert base coordinates to coordinates
@@ -1002,6 +979,65 @@ def collision(self, left_angles, right_angles):
1002979

1003980
return self._mesg(ProtocolCode.COLLISION, 0, degrees1, degrees2, has_reply = True)
1004981

982+
def get_base_coord(self, id):
983+
"""Get the base coordinates of the single arm
984+
985+
Args:
986+
id: 1/2 (L/R)
987+
"""
988+
return self._mesg(ProtocolCode.GET_BASE_COORD, id, has_reply = True)
989+
990+
def write_base_coord(self, id, axis, coord, speed):
991+
"""Base single coordinate movement
992+
993+
Args:
994+
id: 1/2 (L/R)
995+
axis: 1 - 6 (x/y/z/rx/ry/rz)
996+
coord: Coordinate value
997+
speed: 1 - 100
998+
"""
999+
value = self._coord2int(coord) if axis <= 3 else self._angle2int(coord)
1000+
return self._mesg(ProtocolCode.WRITE_BASE_COORD, id, axis, [value], speed)
1001+
1002+
def write_base_coords(self, id, coords, speed):
1003+
"""base coordinate move
1004+
1005+
Args:
1006+
id: 1/2 (L/R)
1007+
coords: coords: a list of coords value(List[float]), length 6, [x(mm), y, z, rx(angle), ry, rz]
1008+
speed: 1 - 100
1009+
"""
1010+
coord_list = []
1011+
for idx in range(3):
1012+
coord_list.append(self._coord2int(coords[idx]))
1013+
for angle in coords[3:]:
1014+
coord_list.append(self._angle2int(angle))
1015+
return self._mesg(ProtocolCode.WRITE_BASE_COORDS, id, coord_list, speed)
1016+
1017+
def jog_inc_coord(self, axis, increment, speed):
1018+
"""Double-arm coordinated coordinate stepping
1019+
1020+
Args:
1021+
axis: 1 - 6 (x/y/z/rx/ry/rz)
1022+
increment:
1023+
speed: 1 - 100
1024+
"""
1025+
value = self._coord2int(increment) if axis <= 3 else self._angle2int(increment)
1026+
return self._mesg(ProtocolCode.JOG_INC_COORD, 0, [value], speed)
1027+
1028+
def collision_switch(self, state):
1029+
"""Collision Detection Switch
1030+
1031+
Args:
1032+
state (int): 0 - close 1 - open (Off by default)
1033+
"""
1034+
return self._mesg(ProtocolCode.COLLISION_SWITCH, state)
1035+
1036+
def is_collision_on(self):
1037+
"""Get collision detection status"""
1038+
return self._mesg(ProtocolCode.IS_COLLISION_ON, 0, has_reply = True)
1039+
1040+
10051041
# def init_iic(self):
10061042
# from smbus2 import SMBus
10071043
# i2c = SMBus(1) # 1 代表 /dev/i2c-1

‎pymycobot/__init__.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@
3030
"MyPalletizerLite"
3131
]
3232

33-
__version__ = "2.9.0b3"
33+
__version__ = "2.9.0b6"
3434
__author__ = "Elephantrobotics"
3535
__email__ = "[email protected]"
3636
__git_url__ = "https://github.com/elephantrobotics/pymycobot"

‎pymycobot/bluet.py‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,19 @@
11
# coding: utf-8
2-
import bluetooth
32
import sys
43

54
class BluetoothConnection:
65
def __init__(self, bd_address=None, port=None):
76
self.device = []
7+
import bluetooth
8+
self.bluetooth = bluetooth
89
self.target_name = "mybuddy"
910
self.nearby_devices = None
1011
self.bd_address = bd_address
1112
self.port = port
1213

1314
def find_target_device(self):
1415
available_addr = []
15-
self.nearby_devices = bluetooth.discover_devices(lookup_names=True, duration=5)
16+
self.nearby_devices = self.bluetooth.discover_devices(lookup_names=True, duration=5)
1617
if self.nearby_devices:
1718
for addr, name in self.nearby_devices:
1819
if self.target_name == name:
@@ -22,7 +23,7 @@ def find_target_device(self):
2223

2324
def connect_target_device(self):
2425
if self.bd_address:
25-
sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
26+
sock = self.bluetooth.BluetoothSocket(self.bluetooth.RFCOMM)
2627
sock.connect((self.bd_address, self.port))
2728
return sock
2829
target_address = self.find_target_device()
@@ -36,7 +37,7 @@ def connect_target_device(self):
3637
sys.stdout.write(device_info)
3738
choose_device = input("please enter 1-{}:".format(len(target_address)))
3839
target_address = target_address[int(choose_device)-1][0]
39-
sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
40+
sock = self.bluetooth.BluetoothSocket(self.bluetooth.RFCOMM)
4041
try:
4142
sock.connect((target_address[0][0], 1))
4243
return sock

‎pymycobot/common.py‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -147,6 +147,13 @@ class ProtocolCode(object):
147147
GET_BASE_COORDS = 0xF0
148148
BASE_TO_SINGLE_COORDS = 0xF1
149149
COLLISION = 0xF2
150+
GET_BASE_COORD = 0xF3
151+
GET_ALL_BASE_COORDS = 0xF4
152+
WRITE_BASE_COORD = 0xF5
153+
WRITE_BASE_COORDS = 0xF6
154+
JOG_INC_COORD = 0xF7
155+
COLLISION_SWITCH = 0xF8
156+
IS_COLLISION_ON = 0xF9
150157

151158
# IIC
152159
# SET_IIC_STATE = 0xA4
@@ -299,7 +306,7 @@ def write(self, command, method=None):
299306
if method == "socket":
300307
data = b""
301308
if self.rasp:
302-
self.sock.sendall(str(command).encode())
309+
self.sock.send(str(command).encode())
303310
else:
304311
self.sock.sendall(bytes(command))
305312

@@ -315,7 +322,7 @@ def write(self, command, method=None):
315322
break
316323
else:
317324
try:
318-
self.sock.settimeout(0.2)
325+
self.sock.settimeout(1)
319326
data = self.sock.recv(1024)
320327
except:
321328
data = b''

‎pymycobot/mybuddy.py‎

Lines changed: 38 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -160,7 +160,12 @@ def _mesg(self, genre, *args, **kwargs):
160160
elif genre in [ProtocolCode.GET_ANGLES]:
161161
return [self._int2angle(angle) for angle in res]
162162
elif genre in [ProtocolCode.GET_ANGLE]:
163-
return self._process_single(self._int2angle(angle) for angle in res)
163+
return self._int2angle(res[0]) if res else None
164+
elif genre in [ProtocolCode.GET_COORD]:
165+
if real_command[5] < 4:
166+
return self._int2coord(res[0]) if res else None
167+
else:
168+
return self._int2angle(res[0]) if res else None
164169
elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE, ProtocolCode.GET_BASE_COORDS, ProtocolCode.BASE_TO_SINGLE_COORDS]:
165170
if res:
166171
r = []
@@ -203,22 +208,48 @@ def send_radians(self, id, radians, speed):
203208
return self._mesg(ProtocolCode.SEND_ANGLES, id, degrees, speed)
204209

205210
# Basic for raspberry pi.
206-
def gpio_init(self):
207-
"""Init GPIO module, and set BCM mode."""
211+
def set_gpio_mode(self, pin_no, mode):
212+
"""Init GPIO module, and set BCM mode.
213+
214+
Args:
215+
pin_no: (int)pin number.
216+
mode: 0 - input 1 - output
217+
"""
208218
import RPi.GPIO as GPIO # type: ignore
209-
210-
GPIO.setmode(GPIO.BCM)
211219
self.gpio = GPIO
220+
self.gpio.setmode(GPIO.BCM)
221+
if mode == 1:
222+
self.gpio.setup(pin_no, self.gpio.OUT)
223+
else:
224+
self.gpio.setup(pin_no, self.gpio.IN)
225+
212226

213-
def gpio_output(self, pin, v):
227+
def set_gpio_output(self, pin, v):
214228
"""Set GPIO output value.
215229
216230
Args:
217231
pin: (int)pin number.
218232
v: (int) 0 / 1
219233
"""
220-
self.gpio.setup(pin, self.gpio.OUT)
221234
self.gpio.output(pin, v)
235+
236+
def set_gpio_input(self, pin):
237+
"""Set GPIO input value.
238+
239+
Args:
240+
pin: (int)pin number.
241+
"""
242+
self.gpio.input(pin)
243+
244+
def set_gpio_pwm(self, pin, baud, dc):
245+
"""Set GPIO PWM value.
246+
247+
Args:
248+
pin: (int)pin number.
249+
baud: (int) 10 - 1000000
250+
dc: (int) 0 - 100
251+
"""
252+
self.gpio.PWM(pin, baud, dc)
222253

223254
# Other
224255
def wait(self, t):

‎pymycobot/mybuddybluetooth.py‎

Lines changed: 39 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,4 @@
11
# coding: utf-8
2-
import bluetooth
32

43
from pymycobot.Interface import MyBuddyCommandGenerator
54
from pymycobot.common import ProtocolCode, write, read
@@ -9,17 +8,32 @@
98

109
class MyBuddyBlueTooth(MyBuddyCommandGenerator):
1110
"""MyBuddy bluetooth API"""
11+
_write = write
1212
def __init__(self, bt_address=None, port = 10):
13-
"""There is a default Bluetooth search time of 5 seconds"""
13+
"""When bt_address is the default value of None, enter the Bluetooth search mode. There is a default Bluetooth search time of 5 seconds"""
1414
super(MyBuddyBlueTooth).__init__()
1515
self.ble = BluetoothConnection(bt_address, port)
16-
self.ble_obj = self.ble.connect_target_device()
16+
self.sock = self.ble.connect_target_device()
1717

18-
def __read(self):
19-
while True:
20-
data = self.ble_obj.recv(1024)
21-
if data:
22-
return data
18+
def connect(self, serialport="/dev/ttyAMA0", baudrate="1000000", timeout='0.2'):
19+
"""Connect the robot arm through the serial port and baud rate
20+
21+
Args:
22+
serialport: (str) default /dev/ttyAMA0
23+
baudrate: default 1000000
24+
timeout: default 0.1
25+
26+
"""
27+
self.rasp = True
28+
self._write(serialport, "socket")
29+
self._write(baudrate, "socket")
30+
self._write(timeout, "socket")
31+
# self._write([254, 254, 1, 2, 32, 32], "socket")
32+
# self._write([254, 254, 1, 2, 32, 32], "socket")
33+
# self._write([254, 254, 1, 2, 32, 32], "socket")
34+
35+
# self._write(timeout, "socket")
36+
# self._write(timeout, "socket")
2337

2438

2539
def _mesg(self, genre, *args, **kwargs):
@@ -36,13 +50,14 @@ def _mesg(self, genre, *args, **kwargs):
3650
"""
3751
real_command, has_reply = super(
3852
MyBuddyBlueTooth, self)._mesg(genre, *args, **kwargs)
39-
self.ble_obj.write(bytes(self._flatten(real_command)))
53+
data = self._write(self._flatten(real_command), "socket")
4054

41-
if has_reply:
42-
data = self.__read()
55+
if data:
4356
res = self._process_received(data, genre, arm=12)
4457
if genre in [
4558
ProtocolCode.ROBOT_VERSION,
59+
ProtocolCode.SOFTWARE_VERSION,
60+
ProtocolCode.GET_ROBOT_ID,
4661
ProtocolCode.IS_POWER_ON,
4762
ProtocolCode.IS_CONTROLLER_CONNECTED,
4863
ProtocolCode.IS_PAUSED, # TODO have bug: return b''
@@ -67,20 +82,30 @@ def _mesg(self, genre, *args, **kwargs):
6782
return self._process_single(res)
6883
elif genre in [ProtocolCode.GET_ANGLES]:
6984
return [self._int2angle(angle) for angle in res]
70-
elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE]:
85+
elif genre in [ProtocolCode.GET_ANGLE]:
86+
return self._process_single(self._int2angle(angle) for angle in res)
87+
elif genre in [ProtocolCode.GET_COORD]:
88+
if real_command[5] < 4:
89+
return self._int2coord(res[0])
90+
else:
91+
return self._int2angle(res[0])
92+
elif genre in [ProtocolCode.GET_COORDS, ProtocolCode.GET_TOOL_REFERENCE, ProtocolCode.GET_WORLD_REFERENCE, ProtocolCode.GET_BASE_COORDS, ProtocolCode.BASE_TO_SINGLE_COORDS]:
7193
if res:
7294
r = []
7395
for idx in range(3):
7496
r.append(self._int2coord(res[idx]))
7597
for idx in range(3, 6):
7698
r.append(self._int2angle(res[idx]))
7799
return r
78-
else:
100+
else:
79101
return res
80-
elif genre in [ProtocolCode.GET_SERVO_VOLTAGES]:
102+
elif genre in [ProtocolCode.GET_SERVO_VOLTAGES, ProtocolCode.COLLISION]:
81103
return [self._int2coord(angle) for angle in res]
82104
else:
83105
return res
84106
return None
85107

108+
def close(self):
109+
self._write("close","socket")
110+
self.sock.close()
86111

0 commit comments

Comments
 (0)