Joint-python-library 0.0.3__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- acrome_joint/Slave_Device.py +314 -0
- acrome_joint/__init__.py +1 -0
- acrome_joint/joint.py +323 -0
- acrome_joint/serial_port.py +59 -0
- joint_python_library-0.0.3.dist-info/METADATA +33 -0
- joint_python_library-0.0.3.dist-info/RECORD +9 -0
- joint_python_library-0.0.3.dist-info/WHEEL +5 -0
- joint_python_library-0.0.3.dist-info/licenses/LICENSE +674 -0
- joint_python_library-0.0.3.dist-info/top_level.txt +1 -0
|
@@ -0,0 +1,314 @@
|
|
|
1
|
+
## this is master for connected devices.
|
|
2
|
+
import struct
|
|
3
|
+
from crccheck.crc import Crc32Mpeg2 as CRC32
|
|
4
|
+
import time
|
|
5
|
+
import enum
|
|
6
|
+
from acrome_joint.serial_port import SerialPort
|
|
7
|
+
'''
|
|
8
|
+
COMMUNICATION PACKAGE =>
|
|
9
|
+
HEADER, ID, DEVICE_FAMILY, PACKAGE_SIZE, COMMAND, STATUS, .............. DATA ................. , CRC
|
|
10
|
+
'''
|
|
11
|
+
|
|
12
|
+
PING_PACKAGE_SIZE = 10
|
|
13
|
+
|
|
14
|
+
#Classical Device Indexes
|
|
15
|
+
Index_Device_Classical = enum.IntEnum('Index', [
|
|
16
|
+
'Header',
|
|
17
|
+
'DeviceID',
|
|
18
|
+
'DeviceFamily',
|
|
19
|
+
'PackageSize',
|
|
20
|
+
'Command',
|
|
21
|
+
'Status',
|
|
22
|
+
'HardwareVersion',
|
|
23
|
+
'SoftwareVersion',
|
|
24
|
+
'Baudrate',
|
|
25
|
+
], start=0)
|
|
26
|
+
|
|
27
|
+
#Classical_Commands
|
|
28
|
+
class Device_Commands(enum.IntEnum):
|
|
29
|
+
ACK = 0x80
|
|
30
|
+
SYNC = 0x40
|
|
31
|
+
|
|
32
|
+
PING = 0x00
|
|
33
|
+
WRITE = 0x01
|
|
34
|
+
READ = 0x02
|
|
35
|
+
EEPROM_SAVE = 0x03
|
|
36
|
+
ERROR_CLEAR = 0x04
|
|
37
|
+
REBOOT = 0x05
|
|
38
|
+
EEPROM_RESET = 0x06
|
|
39
|
+
BL_JUMP = 0x07
|
|
40
|
+
ENTER_CONFIGURATION = 0x08
|
|
41
|
+
ENTER_OPERATION = 0x09
|
|
42
|
+
ERROR = 0x10
|
|
43
|
+
|
|
44
|
+
ENTER_CONFIGURATION_ACK = 0x80 | 0x08 # ACK | ENTER_CONFIGURATION
|
|
45
|
+
ENTER_OPERATION_ACK = 0x80 | 0x09 # ACK | ENTER_OPERATION
|
|
46
|
+
WRITE_ACK = 0x80 | 0x01 # ACK | WRITE
|
|
47
|
+
WRITE_SYNC = 0x40 | 0x01 # SYNC | WRITE
|
|
48
|
+
EEPROM_SAVE_ACK = 0x80 | 0x03 # ACK | EEPROM_WRITE
|
|
49
|
+
EEPROM_SAVE_SYNC = 0x40 | 0x03 # SYNC | EEPROM_WRITE
|
|
50
|
+
|
|
51
|
+
def set_variables_directly(header:int, device_family:int, id:int, status:int=0, *idx_val_pairs, ack = False, port:SerialPort):
|
|
52
|
+
# returns : did ACK come?
|
|
53
|
+
port._ph.flushInput()
|
|
54
|
+
|
|
55
|
+
fmt_str = '<BBBBBB'
|
|
56
|
+
var_count = 0
|
|
57
|
+
size = 0
|
|
58
|
+
for one_pair in idx_val_pairs:
|
|
59
|
+
try:
|
|
60
|
+
if len(one_pair) != 2:
|
|
61
|
+
raise ValueError(f"{one_pair} more than a pair! It is not a pair")
|
|
62
|
+
else:
|
|
63
|
+
fmt_str += ('B' + self._vars[one_pair[0]].type())
|
|
64
|
+
var_count+=1
|
|
65
|
+
size += (1 + self._vars[one_pair[0]].size())
|
|
66
|
+
except:
|
|
67
|
+
raise ValueError(f"{one_pair} is not proper pair")
|
|
68
|
+
|
|
69
|
+
flattened_list = [item for sublist in idx_val_pairs for item in sublist]
|
|
70
|
+
|
|
71
|
+
struct_out = list(struct.pack(fmt_str, *[header, id, device_family, size + PING_PACKAGE_SIZE, Device_Commands.WRITE, status, *flattened_list]))
|
|
72
|
+
struct_out = bytes(struct_out) + struct.pack('<' + 'I', CRC32.calc(struct_out))
|
|
73
|
+
ack_size = PING_PACKAGE_SIZE
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
port._ph.write(struct_out)
|
|
77
|
+
raise NotImplementedError()
|
|
78
|
+
|
|
79
|
+
if _read_ack():
|
|
80
|
+
return True
|
|
81
|
+
else:
|
|
82
|
+
return False
|
|
83
|
+
|
|
84
|
+
|
|
85
|
+
class Slave_Device():
|
|
86
|
+
SERIAL_HEADER = 0x55
|
|
87
|
+
_BROADCAST_ID = 0xFF
|
|
88
|
+
|
|
89
|
+
_BATCH_ID = 254
|
|
90
|
+
def __init__(self, id, device_family, variables, port:SerialPort):
|
|
91
|
+
self._port = port
|
|
92
|
+
self._header = self.SERIAL_HEADER
|
|
93
|
+
self._id = id
|
|
94
|
+
self._device_family = device_family
|
|
95
|
+
self._vars = variables
|
|
96
|
+
self._ack_size = 0
|
|
97
|
+
self.__post_sleep = 0.01
|
|
98
|
+
self.__device_init_sleep = 3
|
|
99
|
+
self.write_ack_enable = False
|
|
100
|
+
|
|
101
|
+
def enable_get_ack(self):
|
|
102
|
+
self.write_ack_enable = True
|
|
103
|
+
|
|
104
|
+
def _init_sleep(self):
|
|
105
|
+
time.sleep(self.__device_init_sleep)
|
|
106
|
+
|
|
107
|
+
def _post_sleep(self):
|
|
108
|
+
time.sleep(self.__post_sleep)
|
|
109
|
+
|
|
110
|
+
def _write_port(self, data):
|
|
111
|
+
self._port._write_bus(data)
|
|
112
|
+
|
|
113
|
+
def _read_port(self, size) -> bytes:
|
|
114
|
+
return self._port._read_bus(size=size)
|
|
115
|
+
|
|
116
|
+
def _parse_received(self, data):
|
|
117
|
+
id = data[Index_Device_Classical.DeviceID]
|
|
118
|
+
data = data[4:-4]
|
|
119
|
+
fmt_str = '<'
|
|
120
|
+
|
|
121
|
+
i = 0
|
|
122
|
+
while i < len(data):
|
|
123
|
+
fmt_str += 'B' + self._vars[data[i]].type()
|
|
124
|
+
i += self._vars[data[i]].size() + 1
|
|
125
|
+
|
|
126
|
+
unpacked = list(struct.unpack(fmt_str, data))
|
|
127
|
+
grouped = zip(*(iter(unpacked),) * 2)
|
|
128
|
+
for group in grouped:
|
|
129
|
+
self._vars[group[0]].value(group[1])
|
|
130
|
+
|
|
131
|
+
def _read_ack(self) -> bool:
|
|
132
|
+
ret = self._read_port(self._ack_size)
|
|
133
|
+
if (ret == "True for test"):
|
|
134
|
+
return True
|
|
135
|
+
if(ret==None):
|
|
136
|
+
return False
|
|
137
|
+
if len(ret) == self._ack_size:
|
|
138
|
+
if (CRC32.calc(ret[:-4]) == struct.unpack('<I', ret[-4:])[0]):
|
|
139
|
+
if ret[Index_Device_Classical.PackageSize] > PING_PACKAGE_SIZE:
|
|
140
|
+
self._parse_received(ret)
|
|
141
|
+
return True
|
|
142
|
+
else:
|
|
143
|
+
return True # ping islemi ve WRITE_ACK icin.
|
|
144
|
+
else:
|
|
145
|
+
return False
|
|
146
|
+
else:
|
|
147
|
+
return False
|
|
148
|
+
|
|
149
|
+
def _read_var_no_timeout(self):
|
|
150
|
+
self._port._no_timeout()
|
|
151
|
+
ack_flag = self._read_ack()
|
|
152
|
+
self._port.set_timeout(0.01)
|
|
153
|
+
if ack_flag:
|
|
154
|
+
return True
|
|
155
|
+
else:
|
|
156
|
+
return False
|
|
157
|
+
|
|
158
|
+
def _pure_command_send(self, command:int):
|
|
159
|
+
fmt_str = '<BBBBBB'
|
|
160
|
+
struct_out = list(struct.pack(fmt_str, *[self._header, self._id, self._device_family, PING_PACKAGE_SIZE, command, 0]))
|
|
161
|
+
struct_out = bytes(struct_out) + struct.pack('<I', CRC32.calc(struct_out))
|
|
162
|
+
self._write_port(struct_out)
|
|
163
|
+
return struct_out
|
|
164
|
+
|
|
165
|
+
def ping(self):
|
|
166
|
+
self._pure_command_send(Device_Commands.PING)
|
|
167
|
+
self._ack_size = PING_PACKAGE_SIZE
|
|
168
|
+
if self._read_ack():
|
|
169
|
+
return True
|
|
170
|
+
else:
|
|
171
|
+
return False
|
|
172
|
+
|
|
173
|
+
def get_variables(self, *indexes):
|
|
174
|
+
self._ack_size = 0
|
|
175
|
+
fmt_str = '<BBBBBB'+'B'*len(indexes)
|
|
176
|
+
|
|
177
|
+
struct_out = list(struct.pack(fmt_str, *[self._header, self._id, self._device_family ,len(indexes) + PING_PACKAGE_SIZE, Device_Commands.READ, 0, *indexes]))
|
|
178
|
+
struct_out = bytes(struct_out) + struct.pack('<' + 'I', CRC32.calc(struct_out))
|
|
179
|
+
for i in indexes:
|
|
180
|
+
self._ack_size += (self._vars[int(i)].size() + 1)
|
|
181
|
+
self._ack_size += PING_PACKAGE_SIZE
|
|
182
|
+
|
|
183
|
+
self._write_port(struct_out)
|
|
184
|
+
|
|
185
|
+
if self._read_ack():
|
|
186
|
+
return [self._vars[index].value() for index in indexes]
|
|
187
|
+
else:
|
|
188
|
+
return [None]
|
|
189
|
+
|
|
190
|
+
def set_variables(self, *idx_val_pairs, ack = False):
|
|
191
|
+
# returns : did ACK come?
|
|
192
|
+
fmt_str = '<BBBBBB'
|
|
193
|
+
var_count = 0
|
|
194
|
+
size = 0
|
|
195
|
+
for one_pair in idx_val_pairs:
|
|
196
|
+
try:
|
|
197
|
+
if len(one_pair) != 2:
|
|
198
|
+
raise ValueError(f"{one_pair} more than a pair! It is not a pair")
|
|
199
|
+
else:
|
|
200
|
+
fmt_str += ('B' + self._vars[one_pair[0]].type())
|
|
201
|
+
var_count+=1
|
|
202
|
+
size += (1 + self._vars[one_pair[0]].size())
|
|
203
|
+
except:
|
|
204
|
+
raise ValueError(f"{one_pair} is not proper pair")
|
|
205
|
+
|
|
206
|
+
flattened_list = [item for sublist in idx_val_pairs for item in sublist]
|
|
207
|
+
|
|
208
|
+
struct_out = list(struct.pack(fmt_str, *[self._header, self._id, self._device_family, size + PING_PACKAGE_SIZE, Device_Commands.WRITE, 0, *flattened_list]))
|
|
209
|
+
struct_out = bytes(struct_out) + struct.pack('<' + 'I', CRC32.calc(struct_out))
|
|
210
|
+
self._ack_size = PING_PACKAGE_SIZE
|
|
211
|
+
|
|
212
|
+
self._write_port(struct_out)
|
|
213
|
+
if(self.write_ack_enable):
|
|
214
|
+
if self._read_ack():
|
|
215
|
+
return True
|
|
216
|
+
else:
|
|
217
|
+
return False
|
|
218
|
+
return False
|
|
219
|
+
|
|
220
|
+
def reboot(self):
|
|
221
|
+
self._pure_command_send(Device_Commands.REBOOT)
|
|
222
|
+
|
|
223
|
+
def eeprom_save(self):
|
|
224
|
+
self._pure_command_send(Device_Commands.EEPROM_SAVE)
|
|
225
|
+
|
|
226
|
+
def factory_reset(self, ack=False):
|
|
227
|
+
self._pure_command_send(Device_Commands.EEPROM_RESET)
|
|
228
|
+
|
|
229
|
+
def enter_bootloader(self):
|
|
230
|
+
self._pure_command_send(Device_Commands.BL_JUMP)
|
|
231
|
+
|
|
232
|
+
def enter_operation(self):
|
|
233
|
+
self._pure_command_send(Device_Commands.ENTER_OPERATION)
|
|
234
|
+
|
|
235
|
+
def enter_configuration(self):
|
|
236
|
+
self._pure_command_send(Device_Commands.ENTER_CONFIGURATION)
|
|
237
|
+
|
|
238
|
+
def get_driver_info(self):
|
|
239
|
+
""" Get hardware and software versions from the driver
|
|
240
|
+
|
|
241
|
+
Args:
|
|
242
|
+
id (int): The device ID of the driver.
|
|
243
|
+
|
|
244
|
+
Returns:
|
|
245
|
+
dict | None: Dictionary containing versions or None.
|
|
246
|
+
"""
|
|
247
|
+
st = dict()
|
|
248
|
+
data = self.get_variables([Index_Device_Classical.HardwareVersion, Index_Device_Classical.SoftwareVersion])
|
|
249
|
+
if data is not None:
|
|
250
|
+
ver = list(struct.pack('<I', data[0]))
|
|
251
|
+
st['HardwareVersion'] = "v{1}.{2}.{3}".format(*ver[::-1])
|
|
252
|
+
ver = list(struct.pack('<I', data[1]))
|
|
253
|
+
st['SoftwareVersion'] = "v{1}.{2}.{3}".format(*ver[::-1])
|
|
254
|
+
|
|
255
|
+
self.__driver_list[id]._config = st
|
|
256
|
+
return st
|
|
257
|
+
else:
|
|
258
|
+
return None
|
|
259
|
+
|
|
260
|
+
def update_driver_id(self, id: int, id_new: int):
|
|
261
|
+
""" Update the device ID of the driver
|
|
262
|
+
|
|
263
|
+
Args:
|
|
264
|
+
id (int): The device ID of the driver
|
|
265
|
+
id_new (int): New device ID
|
|
266
|
+
|
|
267
|
+
Raises:
|
|
268
|
+
ValueError: Current or updating device IDs are not valid
|
|
269
|
+
"""
|
|
270
|
+
if (id < 0) or (id > 254):
|
|
271
|
+
raise ValueError("{} is not a valid ID!".format(id))
|
|
272
|
+
|
|
273
|
+
if (id_new < 0) or (id_new > 254):
|
|
274
|
+
raise ValueError("{} is not a valid ID argument!".format(id_new))
|
|
275
|
+
|
|
276
|
+
self.set_variables([Index_Device_Classical.DeviceID, id_new])
|
|
277
|
+
self._post_sleep()
|
|
278
|
+
|
|
279
|
+
self.eeprom_save(id_new)
|
|
280
|
+
self._post_sleep()
|
|
281
|
+
self.reboot(id)
|
|
282
|
+
|
|
283
|
+
def get_all_variable(self):
|
|
284
|
+
for i in range(0, len(self._vars), 10):
|
|
285
|
+
j = i
|
|
286
|
+
k = min(i + 9, len(self._vars) - 1) # Son grupta sınırlamayı sağlar
|
|
287
|
+
index_list = list(range(j, k + 1))
|
|
288
|
+
self.read_var(*index_list) # her birisi maksimum data sayisiymis gibi dusunerek yazarsak 4 byte olur. her bir pakette 10 adet alsin. maksimuma vurmak istemedigimizden dolayi.
|
|
289
|
+
|
|
290
|
+
class Data_():
|
|
291
|
+
def __init__(self, index, var_type, rw=True, value = 0):
|
|
292
|
+
self.__index = index
|
|
293
|
+
self.__type = var_type
|
|
294
|
+
self.__size = struct.calcsize(self.__type)
|
|
295
|
+
self.__value = value
|
|
296
|
+
self.__rw = rw
|
|
297
|
+
|
|
298
|
+
def value(self, value=None):
|
|
299
|
+
if value is None:
|
|
300
|
+
return self.__value
|
|
301
|
+
elif self.__rw:
|
|
302
|
+
self.__value = struct.unpack('<' + self.__type, struct.pack('<' + self.__type, value))[0]
|
|
303
|
+
|
|
304
|
+
def index(self) ->enum.IntEnum:
|
|
305
|
+
return self.__index
|
|
306
|
+
|
|
307
|
+
def writeable(self) -> bool:
|
|
308
|
+
return self.__rw
|
|
309
|
+
|
|
310
|
+
def size(self) -> int:
|
|
311
|
+
return self.__size
|
|
312
|
+
|
|
313
|
+
def type(self) -> str:
|
|
314
|
+
return self.__type
|
acrome_joint/__init__.py
ADDED
|
@@ -0,0 +1 @@
|
|
|
1
|
+
from joint import *
|
acrome_joint/joint.py
ADDED
|
@@ -0,0 +1,323 @@
|
|
|
1
|
+
import struct
|
|
2
|
+
from crccheck.crc import Crc32Mpeg2 as CRC32
|
|
3
|
+
import serial
|
|
4
|
+
import time
|
|
5
|
+
from packaging.version import parse as parse_version
|
|
6
|
+
import requests
|
|
7
|
+
import hashlib
|
|
8
|
+
import tempfile
|
|
9
|
+
from stm32loader.main import main as stm32loader_main
|
|
10
|
+
import enum
|
|
11
|
+
from acrome_joint.Slave_Device import *
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
# enter here for extra commands:
|
|
15
|
+
#class Device_ExtraCommands(enum.IntEnum):
|
|
16
|
+
# .......... start with 11
|
|
17
|
+
# .......... end of extra commmands max: 39
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
Index_Joint = enum.IntEnum('Index', [
|
|
21
|
+
'Header',
|
|
22
|
+
'DeviceID',
|
|
23
|
+
'DeviceFamily',
|
|
24
|
+
'PackageSize',
|
|
25
|
+
'Command',
|
|
26
|
+
'Status',
|
|
27
|
+
'HardwareVersion',
|
|
28
|
+
'SoftwareVersion',
|
|
29
|
+
'Baudrate', #'WritableStart' = iBaudrate
|
|
30
|
+
# user parameter start
|
|
31
|
+
'OperationMode',
|
|
32
|
+
'Enable',
|
|
33
|
+
'Vbus_read',
|
|
34
|
+
'Temprature_read',
|
|
35
|
+
'currentId_loop_kp',
|
|
36
|
+
'currentId_loop_ki',
|
|
37
|
+
'currentId_loop_kd',
|
|
38
|
+
'currentIq_loop_kp',
|
|
39
|
+
'currentIq_loop_ki',
|
|
40
|
+
'currentIq_loop_kd',
|
|
41
|
+
'velocity_loop_kp',
|
|
42
|
+
'velocity_loop_ki',
|
|
43
|
+
'velocity_loop_kd',
|
|
44
|
+
'position_loop_kp',
|
|
45
|
+
'position_loop_ki',
|
|
46
|
+
'position_loop_kd',
|
|
47
|
+
'max_position',
|
|
48
|
+
'min_position',
|
|
49
|
+
'max_velocity',
|
|
50
|
+
'max_current',
|
|
51
|
+
'current_Va',
|
|
52
|
+
'current_Vb',
|
|
53
|
+
'current_Vc',
|
|
54
|
+
'current_Ia',
|
|
55
|
+
'current_Ib',
|
|
56
|
+
'current_Ic',
|
|
57
|
+
'current_Id',
|
|
58
|
+
'current_Iq',
|
|
59
|
+
'current_velocity',
|
|
60
|
+
'current_position',
|
|
61
|
+
'current_electrical_degree',
|
|
62
|
+
'current_electrical_radian',
|
|
63
|
+
'setpoint_current',
|
|
64
|
+
'setpoint_velocity',
|
|
65
|
+
'setpoint_position',
|
|
66
|
+
'openloop_voltage_size',
|
|
67
|
+
'openloop_angle_degree',
|
|
68
|
+
'current_lock_angle_degree',
|
|
69
|
+
# user parameter end
|
|
70
|
+
'Config_TimeStamp',
|
|
71
|
+
'Config_Description',
|
|
72
|
+
'CRCValue',
|
|
73
|
+
], start=0)
|
|
74
|
+
|
|
75
|
+
|
|
76
|
+
|
|
77
|
+
def scan_blue_devices(port:SerialPort):
|
|
78
|
+
device = Blue(0, port)
|
|
79
|
+
available_devices = []
|
|
80
|
+
|
|
81
|
+
for id in range(0,255):
|
|
82
|
+
device._id = id
|
|
83
|
+
if(device.ping()):
|
|
84
|
+
available_devices.append(id)
|
|
85
|
+
|
|
86
|
+
return available_devices
|
|
87
|
+
|
|
88
|
+
|
|
89
|
+
class Joint(Slave_Device):
|
|
90
|
+
_PRODUCT_TYPE = 0xDA
|
|
91
|
+
_PACKAGE_ESSENTIAL_SIZE = 6
|
|
92
|
+
_STATUS_KEY_LIST = ['EEPROM', 'Software Version', 'Hardware Version']
|
|
93
|
+
__RELEASE_URL = "https://api.github.com/repos/AAcrome-Smart-Motion-Devices/SMD-Blue-Firmware/releases/{version}"
|
|
94
|
+
|
|
95
|
+
class Operation_Mode():
|
|
96
|
+
OPERATION_OPENLOOP = 0
|
|
97
|
+
OPERATION_CURRENT_LOCK = 1
|
|
98
|
+
OPERATION_FOC_TORQUE = 2
|
|
99
|
+
OPERATION_FOC_VELOCITY = 3
|
|
100
|
+
OPERATION_FOC_POSITION = 4
|
|
101
|
+
|
|
102
|
+
def __init__(self, ID, port:SerialPort) -> bool:
|
|
103
|
+
self.__ack_size = 0
|
|
104
|
+
if ID > 254 or ID < 0:
|
|
105
|
+
raise ValueError("Device ID can not be higher than 254 or lower than 0!")
|
|
106
|
+
device_special_data = [
|
|
107
|
+
Data_(Index_Joint.Header, 'B', False, 0x55),
|
|
108
|
+
Data_(Index_Joint.DeviceID, 'B'),
|
|
109
|
+
Data_(Index_Joint.DeviceFamily, 'B'),
|
|
110
|
+
Data_(Index_Joint.PackageSize, 'B'),
|
|
111
|
+
Data_(Index_Joint.Command, 'B'),
|
|
112
|
+
Data_(Index_Joint.Status, 'B'),
|
|
113
|
+
Data_(Index_Joint.HardwareVersion, 'I'),
|
|
114
|
+
Data_(Index_Joint.SoftwareVersion, 'I'),
|
|
115
|
+
Data_(Index_Joint.Baudrate, 'I'),
|
|
116
|
+
# user parameter starts
|
|
117
|
+
Data_(Index_Joint.OperationMode, 'B'),
|
|
118
|
+
Data_(Index_Joint.Enable, 'B'),
|
|
119
|
+
Data_(Index_Joint.Vbus_read, 'f'),
|
|
120
|
+
Data_(Index_Joint.Temprature_read, 'f'),
|
|
121
|
+
Data_(Index_Joint.currentId_loop_kp, 'f'),
|
|
122
|
+
Data_(Index_Joint.currentId_loop_ki, 'f'),
|
|
123
|
+
Data_(Index_Joint.currentId_loop_kd, 'f'),
|
|
124
|
+
Data_(Index_Joint.currentIq_loop_kp, 'f'),
|
|
125
|
+
Data_(Index_Joint.currentIq_loop_ki, 'f'),
|
|
126
|
+
Data_(Index_Joint.currentIq_loop_kd, 'f'),
|
|
127
|
+
Data_(Index_Joint.velocity_loop_kp, 'f'),
|
|
128
|
+
Data_(Index_Joint.velocity_loop_ki, 'f'),
|
|
129
|
+
Data_(Index_Joint.velocity_loop_kd, 'f'),
|
|
130
|
+
Data_(Index_Joint.position_loop_kp, 'f'),
|
|
131
|
+
Data_(Index_Joint.position_loop_ki, 'f'),
|
|
132
|
+
Data_(Index_Joint.position_loop_kd, 'f'),
|
|
133
|
+
Data_(Index_Joint.max_position, 'i'),
|
|
134
|
+
Data_(Index_Joint.min_position, 'i'),
|
|
135
|
+
Data_(Index_Joint.max_velocity, 'f'),
|
|
136
|
+
Data_(Index_Joint.max_current, 'f'),
|
|
137
|
+
Data_(Index_Joint.current_Va, 'f'),
|
|
138
|
+
Data_(Index_Joint.current_Vb, 'f'),
|
|
139
|
+
Data_(Index_Joint.current_Vc, 'f'),
|
|
140
|
+
Data_(Index_Joint.current_Ia, 'f'),
|
|
141
|
+
Data_(Index_Joint.current_Ib, 'f'),
|
|
142
|
+
Data_(Index_Joint.current_Ic, 'f'),
|
|
143
|
+
Data_(Index_Joint.current_Id, 'f'),
|
|
144
|
+
Data_(Index_Joint.current_Iq, 'f'),
|
|
145
|
+
Data_(Index_Joint.current_velocity, 'f'),
|
|
146
|
+
Data_(Index_Joint.current_position, 'i'),
|
|
147
|
+
Data_(Index_Joint.current_electrical_degree, 'f'),
|
|
148
|
+
Data_(Index_Joint.current_electrical_radian, 'f'),
|
|
149
|
+
Data_(Index_Joint.setpoint_current, 'f'),
|
|
150
|
+
Data_(Index_Joint.setpoint_velocity, 'f'),
|
|
151
|
+
Data_(Index_Joint.setpoint_position, 'f'),
|
|
152
|
+
Data_(Index_Joint.openloop_voltage_size, 'f'),
|
|
153
|
+
Data_(Index_Joint.openloop_angle_degree, 'f'),
|
|
154
|
+
Data_(Index_Joint.current_lock_angle_degree, 'f'),
|
|
155
|
+
# user parameter end
|
|
156
|
+
Data_(Index_Joint.Config_TimeStamp, 'Q'),
|
|
157
|
+
Data_(Index_Joint.Config_Description, '100s'),
|
|
158
|
+
Data_(Index_Joint.CRCValue, 'I'),
|
|
159
|
+
]
|
|
160
|
+
super().__init__(ID, self._PRODUCT_TYPE, device_special_data, port)
|
|
161
|
+
self._vars[Index_Joint.DeviceID].value(ID)
|
|
162
|
+
|
|
163
|
+
# user start for extra commands.
|
|
164
|
+
#def command(self):
|
|
165
|
+
|
|
166
|
+
def __del__(self):
|
|
167
|
+
pass
|
|
168
|
+
|
|
169
|
+
def get_latest_fw_version(self):
|
|
170
|
+
""" Get the latest firmware version from the Github servers.
|
|
171
|
+
|
|
172
|
+
Returns:
|
|
173
|
+
String: Latest firmware version
|
|
174
|
+
"""
|
|
175
|
+
response = requests.get(url=self.__class__.__RELEASE_URL.format(version='latest'))
|
|
176
|
+
if (response.status_code in [200, 302]):
|
|
177
|
+
return (response.json()['tag_name'])
|
|
178
|
+
|
|
179
|
+
def update_fw_version(self, version=''):
|
|
180
|
+
""" Update firmware version with respect to given version string.
|
|
181
|
+
|
|
182
|
+
Args:
|
|
183
|
+
id (int): The device ID of the driver
|
|
184
|
+
version (str, optional): Desired firmware version. Defaults to ''.
|
|
185
|
+
|
|
186
|
+
Returns:
|
|
187
|
+
Bool: True if the firmware is updated
|
|
188
|
+
"""
|
|
189
|
+
|
|
190
|
+
fw_file = tempfile.NamedTemporaryFile("wb+",delete=False)
|
|
191
|
+
if version == '':
|
|
192
|
+
version = 'latest'
|
|
193
|
+
else:
|
|
194
|
+
version = 'tags/' + version
|
|
195
|
+
|
|
196
|
+
response = requests.get(url=self.__class__.__RELEASE_URL.format(version=version))
|
|
197
|
+
if response.status_code in [200, 302]:
|
|
198
|
+
assets = response.json()['assets']
|
|
199
|
+
|
|
200
|
+
fw_dl_url = None
|
|
201
|
+
md5_dl_url = None
|
|
202
|
+
for asset in assets:
|
|
203
|
+
if '.bin' in asset['name']:
|
|
204
|
+
fw_dl_url = asset['browser_download_url']
|
|
205
|
+
elif '.md5' in asset['name']:
|
|
206
|
+
md5_dl_url = asset['browser_download_url']
|
|
207
|
+
|
|
208
|
+
if None in [fw_dl_url, md5_dl_url]:
|
|
209
|
+
raise Exception("Could not found requested firmware file! Check your connection to GitHub.")
|
|
210
|
+
|
|
211
|
+
# Get binary firmware file
|
|
212
|
+
md5_fw = None
|
|
213
|
+
response = requests.get(fw_dl_url, stream=True)
|
|
214
|
+
if (response.status_code in [200, 302]):
|
|
215
|
+
fw_file.write(response.content)
|
|
216
|
+
md5_fw = hashlib.md5(response.content).hexdigest()
|
|
217
|
+
else:
|
|
218
|
+
raise Exception("Could not fetch requested binary file! Check your connection to GitHub.")
|
|
219
|
+
|
|
220
|
+
# Get MD5 file
|
|
221
|
+
response = requests.get(md5_dl_url, stream=True)
|
|
222
|
+
if (response.status_code in [200, 302]):
|
|
223
|
+
md5_retreived = response.text.split(' ')[0]
|
|
224
|
+
if (md5_fw == md5_retreived):
|
|
225
|
+
|
|
226
|
+
# Put the driver in to bootloader mode
|
|
227
|
+
self.enter_bootloader()
|
|
228
|
+
time.sleep(0.1)
|
|
229
|
+
|
|
230
|
+
# Close serial port
|
|
231
|
+
serial_settings = self._port._ph.get_settings()
|
|
232
|
+
self._port._ph.close()
|
|
233
|
+
|
|
234
|
+
# Upload binary
|
|
235
|
+
args = ['-p', self._port._ph.portstr, '-b', str(115200), '-e', '-w', '-v', fw_file.name]
|
|
236
|
+
stm32loader_main(*args)
|
|
237
|
+
|
|
238
|
+
# Delete uploaded binary
|
|
239
|
+
if (not fw_file.closed):
|
|
240
|
+
fw_file.close()
|
|
241
|
+
|
|
242
|
+
# Re open port to the user with saved settings
|
|
243
|
+
self._port._ph.apply_settings(serial_settings)
|
|
244
|
+
self._port._ph.open()
|
|
245
|
+
return True
|
|
246
|
+
|
|
247
|
+
else:
|
|
248
|
+
raise Exception("MD5 Mismatch!")
|
|
249
|
+
else:
|
|
250
|
+
raise Exception("Could not fetch requested MD5 file! Check your connection to GitHub.")
|
|
251
|
+
else:
|
|
252
|
+
raise Exception("Could not found requested firmware files list! Check your connection to GitHub.")
|
|
253
|
+
|
|
254
|
+
def enable_torque(self, en: bool):
|
|
255
|
+
""" Enable power to the motor of the driver.
|
|
256
|
+
|
|
257
|
+
Args:
|
|
258
|
+
id (int): The device ID of the driver
|
|
259
|
+
en (bool): Enable. True enables the torque.
|
|
260
|
+
"""
|
|
261
|
+
|
|
262
|
+
self.set_variables([Index_Joint.Enable, en])
|
|
263
|
+
self._post_sleep()
|
|
264
|
+
|
|
265
|
+
def set_config_timeStamp(self):
|
|
266
|
+
epoch_seconds = int(time.time())
|
|
267
|
+
self.set_variables([Index_Joint.Config_TimeStamp, epoch_seconds])
|
|
268
|
+
self._post_sleep()
|
|
269
|
+
|
|
270
|
+
def set_config_description(self, description:str):
|
|
271
|
+
if len(description) >= 100:
|
|
272
|
+
text = description[:99] + '\0'
|
|
273
|
+
else:
|
|
274
|
+
text = description + '\0'
|
|
275
|
+
text = text.ljust(100, ' ')
|
|
276
|
+
text = text.encode('ascii') # veya utf-8 eğer uyumluysa
|
|
277
|
+
|
|
278
|
+
self.set_variables([Index_Joint.Config_Description, text])
|
|
279
|
+
self._post_sleep()
|
|
280
|
+
|
|
281
|
+
|
|
282
|
+
|
|
283
|
+
def get_FOC_parameters(self, package_number:int):
|
|
284
|
+
if package_number >= 4:
|
|
285
|
+
raise "invalid package number ex: 0, 1, 2"
|
|
286
|
+
classic_package = [
|
|
287
|
+
Index_Joint.Enable,
|
|
288
|
+
Index_Joint.current_Id, Index_Joint.current_Iq,
|
|
289
|
+
Index_Joint.current_velocity, Index_Joint.current_position,
|
|
290
|
+
Index_Joint.Temprature_read,
|
|
291
|
+
Index_Joint.setpoint_current, Index_Joint.setpoint_velocity, Index_Joint.setpoint_position
|
|
292
|
+
]
|
|
293
|
+
package_0 = [
|
|
294
|
+
Index_Joint.currentId_loop_kp,
|
|
295
|
+
Index_Joint.currentId_loop_ki,
|
|
296
|
+
Index_Joint.currentId_loop_kd,
|
|
297
|
+
Index_Joint.currentIq_loop_kp,
|
|
298
|
+
Index_Joint.currentIq_loop_ki,
|
|
299
|
+
Index_Joint.currentIq_loop_kd,
|
|
300
|
+
]
|
|
301
|
+
package_1 = [
|
|
302
|
+
Index_Joint.velocity_loop_kp,
|
|
303
|
+
Index_Joint.velocity_loop_ki,
|
|
304
|
+
Index_Joint.velocity_loop_kd,
|
|
305
|
+
]
|
|
306
|
+
package_2 = [
|
|
307
|
+
Index_Joint.position_loop_kp,
|
|
308
|
+
Index_Joint.position_loop_ki,
|
|
309
|
+
Index_Joint.position_loop_kd,
|
|
310
|
+
]
|
|
311
|
+
|
|
312
|
+
if package_number == 0:
|
|
313
|
+
return self.get_variables(*classic_package , *package_0)
|
|
314
|
+
|
|
315
|
+
elif package_number == 1:
|
|
316
|
+
return self.get_variables(*classic_package , *package_1)
|
|
317
|
+
|
|
318
|
+
elif package_number == 2:
|
|
319
|
+
return self.get_variables(*classic_package , *package_2)
|
|
320
|
+
|
|
321
|
+
elif package_number == 3:
|
|
322
|
+
return self.get_variables(*classic_package)
|
|
323
|
+
|
|
@@ -0,0 +1,59 @@
|
|
|
1
|
+
import serial
|
|
2
|
+
|
|
3
|
+
class SerialPort:
|
|
4
|
+
def __init__(self, port_name, baudrate=921600, timeout=0.1, isTest:bool=False):
|
|
5
|
+
self.port_name = port_name
|
|
6
|
+
self.baudrate = baudrate
|
|
7
|
+
self.timeout = timeout
|
|
8
|
+
self.isTest = isTest
|
|
9
|
+
if(self.isTest != True):
|
|
10
|
+
self._ph = serial.Serial(port=self.port_name, baudrate=self.baudrate, timeout=self.timeout)
|
|
11
|
+
|
|
12
|
+
def close_port(self):
|
|
13
|
+
if self.isTest:
|
|
14
|
+
print(f"virtual port destroyed.")
|
|
15
|
+
else:
|
|
16
|
+
if self._ph and self._ph.is_open:
|
|
17
|
+
self._ph.reset_input_buffer()
|
|
18
|
+
self._ph.reset_output_buffer()
|
|
19
|
+
self._ph.close()
|
|
20
|
+
print(f"Port '{self.port_name}' shut down.")
|
|
21
|
+
else:
|
|
22
|
+
print(f"Port '{self.port_name}' already closed.")
|
|
23
|
+
|
|
24
|
+
def __del__(self):
|
|
25
|
+
try:
|
|
26
|
+
self.close_port()
|
|
27
|
+
except Exception as e:
|
|
28
|
+
print(f"Port yok edilirken bir hata oluştu: {e}")
|
|
29
|
+
|
|
30
|
+
def _write_bus(self,data):
|
|
31
|
+
if(self.isTest == True):
|
|
32
|
+
print(list(data))
|
|
33
|
+
else:
|
|
34
|
+
self._ph.flushInput()
|
|
35
|
+
self._ph.write(data)
|
|
36
|
+
pass
|
|
37
|
+
|
|
38
|
+
def _read_bus(self, size):
|
|
39
|
+
if(self.isTest == False):
|
|
40
|
+
return self._ph.read(size=size)
|
|
41
|
+
else:
|
|
42
|
+
print("Read Bus(TEST!)")
|
|
43
|
+
return "True for test"
|
|
44
|
+
|
|
45
|
+
def _no_timeout(self):
|
|
46
|
+
notimeout = 50 #in seconds
|
|
47
|
+
if(self.isTest):
|
|
48
|
+
print(f"port timeout update is setted to {notimeout} (TEST!)")
|
|
49
|
+
else:
|
|
50
|
+
self._ph.timeout = notimeout
|
|
51
|
+
|
|
52
|
+
def set_timeout(self, timeout):
|
|
53
|
+
if(self.isTest):
|
|
54
|
+
print(f"port timeout update to {timeout} (TEST!)")
|
|
55
|
+
else:
|
|
56
|
+
self._ph.timeout = timeout
|
|
57
|
+
|
|
58
|
+
|
|
59
|
+
|