├── lewansoul-lx16a-terminal ├── MANIFEST.in ├── scripts │ └── lewansoul_lx16a_terminal ├── setup.py ├── resources │ ├── ConfigureId.ui │ ├── ConfigureMaxTemperature.ui │ ├── ConfigurePositionLimits.ui │ ├── ConfigureVoltageLimits.ui │ ├── ConfigurePositionOffset.ui │ └── ServoTerminal.ui └── lewansoul_lx16a_terminal.py ├── lewansoul-lx16a ├── .gitignore ├── setup.py ├── lewansoul_lx16a_controller.py └── lewansoul_lx16a.py ├── resources └── screenshot-v0.1.png ├── LICENSE └── README.md /lewansoul-lx16a-terminal/MANIFEST.in: -------------------------------------------------------------------------------- 1 | include resources/*.ui 2 | -------------------------------------------------------------------------------- /lewansoul-lx16a/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | dist/ 3 | lewansoul-lx16a.egg-info/ 4 | -------------------------------------------------------------------------------- /resources/screenshot-v0.1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maximkulkin/lewansoul-lx16a/HEAD/resources/screenshot-v0.1.png -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/scripts/lewansoul_lx16a_terminal: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Terminal GUI for LewanSoul LX-16A servos 4 | 5 | Author: Maxim Kulkin 6 | Website: https://github.com/maximkulkin/lewansoul-lx16a 7 | """ 8 | 9 | from lewansoul_lx16a_terminal import main 10 | main() 11 | -------------------------------------------------------------------------------- /lewansoul-lx16a/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | setup( 4 | name='lewansoul_lx16a', 5 | version='0.1', 6 | description='Driver to control LewanSoul LX-16A servos', 7 | author='Maxim Kulkin', 8 | author_email='maxim.kulkin@gmail.com', 9 | url='https://github.com/maximkulkin/lewansoul-lx16a', 10 | py_modules=['lewansoul_lx16a', 'lewansoul_lx16a_controller'], 11 | license='MIT', 12 | classifiers=[ 13 | 'Development Status :: 5 - Production/Stable', 14 | 'Intended Audience :: Developers', 15 | 'License :: OSI Approved :: MIT License', 16 | 'Programming Language :: Python :: 3', 17 | 'Programming Language :: Python :: 3.3', 18 | 'Programming Language :: Python :: 3.4', 19 | 'Programming Language :: Python :: 3.5', 20 | 'Programming Language :: Python :: 3.6', 21 | 'Programming Language :: Python :: Implementation :: CPython', 22 | 'Programming Language :: Python :: Implementation :: PyPy', 23 | ], 24 | install_requires=['pyserial'], 25 | ) 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Maxim Kulkin 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | import os 3 | import os.path 4 | import fnmatch 5 | 6 | def find_files(directory, pattern, recursive=True): 7 | files = [] 8 | for dirpath, dirnames, filenames in os.walk(directory, topdown=True): 9 | if not recursive: 10 | while dirnames: 11 | del dirnames[0] 12 | 13 | for filename in filenames: 14 | filepath = os.path.join(dirpath, filename) 15 | if fnmatch.fnmatch(filepath, pattern): 16 | files.append(filepath) 17 | 18 | return files 19 | 20 | 21 | setup( 22 | name='lewansoul_lx16a_terminal', 23 | version='0.1', 24 | description='GUI to configure and control LewanSoul LX-16A servos', 25 | author='Maxim Kulkin', 26 | author_email='maxim.kulkin@gmail.com', 27 | url='https://github.com/maximkulkin/lewansoul-lx16a', 28 | py_modules=['lewansoul_lx16a_terminal'], 29 | scripts=['scripts/lewansoul_lx16a_terminal'], 30 | data_files=[ 31 | ('resources', find_files('resources', '*.ui')), 32 | ], 33 | include_package_data=True, 34 | license='MIT', 35 | classifiers=[ 36 | 'Development Status :: 5 - Production/Stable', 37 | 'Intended Audience :: Developers', 38 | 'License :: OSI Approved :: MIT License', 39 | 'Programming Language :: Python :: 3', 40 | 'Programming Language :: Python :: 3.3', 41 | 'Programming Language :: Python :: 3.4', 42 | 'Programming Language :: Python :: 3.5', 43 | 'Programming Language :: Python :: 3.6', 44 | 'Programming Language :: Python :: Implementation :: CPython', 45 | 'Programming Language :: Python :: Implementation :: PyPy', 46 | ], 47 | install_requires=['lewansoul_lx16a', 'pyserial', 'PyQt5'], 48 | ) 49 | -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/resources/ConfigureId.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | configureIdDialog 4 | 5 | 6 | 7 | 0 8 | 0 9 | 206 10 | 87 11 | 12 | 13 | 14 | Configure ID 15 | 16 | 17 | 18 | 19 | 20 | New ID 21 | 22 | 23 | 24 | 25 | 26 | 27 | 1 28 | 29 | 30 | 253 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | Qt::Horizontal 40 | 41 | 42 | 43 | 0 44 | 0 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | OK 53 | 54 | 55 | 56 | 57 | 58 | 59 | Cancel 60 | 61 | 62 | 63 | 64 | 65 | 66 | Qt::Horizontal 67 | 68 | 69 | 70 | 0 71 | 0 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | LewanSoul LX-16A servos driver & GUI 2 | ==================================== 3 | 4 | LewanSoul LX-16A is a wonderful servos although using them requires 5 | implementing custom serial protocol and you need to configure them 6 | before use (set unique IDs for all servos, configur limits, etc.). 7 | Company provides a tool for that, but it works only on Windows. 8 | 9 | This project aims to fill the gap for non-Windows users. It consists 10 | of two parts: a servo library and a GUI tool. Servo library implements 11 | an interface to talk to servos using their protocol and GUI provides 12 | convenient way to configure them. 13 | 14 | ![alt text](resources/screenshot-v0.1.png) 15 | 16 | Installation 17 | ============ 18 | Just install both packages to use them: 19 | 20 | ``` 21 | ( cd lewansoul-lx16a && python setup.py install ) 22 | ( cd lewansoul-lx16a-terminal && python setup.py install ) 23 | ``` 24 | 25 | To launch GUI use `lewansoul_lx16a_terminal` command. 26 | 27 | Code example 28 | ============ 29 | 30 | ```python 31 | import serial 32 | import lewansoul_lx16a 33 | 34 | SERIAL_PORT = '/dev/tty.wchusbserial1410' 35 | 36 | controller = lewansoul_lx16a.ServoController( 37 | serial.Serial(SERIAL_PORT, 115200, timeout=1), 38 | ) 39 | 40 | # control servos directly 41 | controller.move(1, 100) # move servo ID=1 to position 100 42 | 43 | # or through proxy objects 44 | servo1 = controller.servo(1) 45 | servo2 = controller.servo(2) 46 | 47 | servo1.move(100) 48 | 49 | # synchronous move of multiple servos 50 | servo1.move_prepare(300) 51 | servo2.move_prepare(600) 52 | controller.move_start() 53 | ``` 54 | 55 | Example of controlling servos through Bus Servo Controller: 56 | ```python 57 | import serial 58 | import time 59 | from lewansoul_lx16a_controller import ServoController 60 | 61 | logging.basicConfig(level=logging.DEBUG) 62 | 63 | s = serial.Serial('/dev/tty.usbserial-00000000', 9600, timeout=2) 64 | c = ServoController(s, timeout=5) 65 | 66 | print(c.get_battery_voltage()) 67 | 68 | servo1_id = 3 69 | servo2_id = 5 70 | 71 | print(c.get_positions([servo1_id, servo2_id])) 72 | 73 | c.move({servo1_id: 1000, servo2_id: 500}, time=300) 74 | time.sleep(0.3) 75 | c.move({servo1_id: 800, servo2_id: 500}, time=2000) 76 | time.sleep(2) 77 | c.unload([servo1_id, servo2_id]) 78 | time.sleep(0.1) # important not to close serial connection immediately 79 | ``` 80 | -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/resources/ConfigureMaxTemperature.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | configureMaxTemperatureDialog 4 | 5 | 6 | 7 | 0 8 | 0 9 | 206 10 | 87 11 | 12 | 13 | 14 | Configure Max Temperature 15 | 16 | 17 | 18 | 19 | 20 | Max Temperature 21 | 22 | 23 | 24 | 25 | 26 | 27 | 50 28 | 29 | 30 | 100 31 | 32 | 33 | 85 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | Qt::Horizontal 43 | 44 | 45 | 46 | 0 47 | 0 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | OK 56 | 57 | 58 | 59 | 60 | 61 | 62 | Cancel 63 | 64 | 65 | 66 | 67 | 68 | 69 | Qt::Horizontal 70 | 71 | 72 | 73 | 0 74 | 0 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/resources/ConfigurePositionLimits.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | configurePositionLimitsDialog 4 | 5 | 6 | 7 | 0 8 | 0 9 | 243 10 | 87 11 | 12 | 13 | 14 | Configure Position Limits 15 | 16 | 17 | 18 | 19 | 20 | Position Limits 21 | 22 | 23 | 24 | 25 | 26 | 27 | 0 28 | 29 | 30 | 1000 31 | 32 | 33 | 34 | 35 | 36 | 37 | 0 38 | 39 | 40 | 1000 41 | 42 | 43 | 1000 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | Qt::Horizontal 53 | 54 | 55 | 56 | 0 57 | 0 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | OK 66 | 67 | 68 | 69 | 70 | 71 | 72 | Cancel 73 | 74 | 75 | 76 | 77 | 78 | 79 | Qt::Horizontal 80 | 81 | 82 | 83 | 0 84 | 0 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/resources/ConfigureVoltageLimits.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | configureVoltageLimitsDialog 4 | 5 | 6 | 7 | 0 8 | 0 9 | 252 10 | 87 11 | 12 | 13 | 14 | Configure Voltage Limits 15 | 16 | 17 | 18 | 19 | 20 | Voltage Limits 21 | 22 | 23 | 24 | 25 | 26 | 27 | 4500 28 | 29 | 30 | 9000 31 | 32 | 33 | 34 | 35 | 36 | 37 | 4500 38 | 39 | 40 | 12000 41 | 42 | 43 | 9000 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | Qt::Horizontal 53 | 54 | 55 | 56 | 0 57 | 0 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | OK 66 | 67 | 68 | 69 | 70 | 71 | 72 | Cancel 73 | 74 | 75 | 76 | 77 | 78 | 79 | Qt::Horizontal 80 | 81 | 82 | 83 | 0 84 | 0 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/resources/ConfigurePositionOffset.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | configurePositionOffsetDialog 4 | 5 | 6 | 7 | 0 8 | 0 9 | 206 10 | 87 11 | 12 | 13 | 14 | Configure Position Offset 15 | 16 | 17 | 18 | 19 | 20 | Position Offset 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 400 31 | 0 32 | 33 | 34 | -125 35 | 125 36 | 0 37 | Qt::Horizontal 38 | QSlider::TicksBelow 39 | 25 40 | 41 | 42 | 43 | 44 | -125 45 | 125 46 | false 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | Qt::Horizontal 57 | 58 | 59 | 60 | 0 61 | 0 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | OK 70 | 71 | 72 | 73 | 74 | 75 | 76 | Cancel 77 | 78 | 79 | 80 | 81 | 82 | 83 | Qt::Horizontal 84 | 85 | 86 | 87 | 0 88 | 0 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /lewansoul-lx16a/lewansoul_lx16a_controller.py: -------------------------------------------------------------------------------- 1 | __all__ = [ 2 | 'ServoController', 3 | 'TimeoutError', 4 | ] 5 | 6 | 7 | from serial.serialutil import Timeout 8 | from functools import partial 9 | from itertools import chain 10 | import threading 11 | import logging 12 | 13 | 14 | CMD_SERVO_MOVE = 3 15 | CMD_ACTION_GROUP_RUN = 6 16 | CMD_ACTION_STOP = 7 17 | CMD_ACTION_SPEED = 11 18 | CMD_GET_BATTERY_VOLTAGE = 15 19 | CMD_MULT_SERVO_UNLOAD = 20 20 | CMD_MULT_SERVO_POS_READ = 21 21 | 22 | 23 | def lower_byte(value): 24 | return int(value) % 256 25 | 26 | 27 | def higher_byte(value): 28 | return int(value / 256) % 256 29 | 30 | 31 | def word(low, high): 32 | return int(low) + int(high)*256 33 | 34 | 35 | def hex_data(data): 36 | return '[%s]' % ', '.join(['0x%02x' % x for x in data]) 37 | 38 | 39 | def clamp(range_min, range_max, value): 40 | return min(range_max, max(range_min, value)) 41 | 42 | 43 | class TimeoutError(RuntimeError): 44 | pass 45 | 46 | 47 | LOGGER = logging.getLogger('lewansoul.servos.lx16a') 48 | 49 | 50 | class ServoController(object): 51 | def __init__(self, serial, timeout=1): 52 | self._serial = serial 53 | self._timeout = timeout 54 | self._lock = threading.RLock() 55 | self._responses = [] 56 | 57 | def _command(self, command, *params): 58 | length = 2 + len(params) 59 | with self._lock: 60 | LOGGER.debug('Sending servo control packet: %s', hex_data([ 61 | 0x55, 0x55, length, command, *params 62 | ])) 63 | self._serial.write(bytearray([ 64 | 0x55, 0x55, length, command, *params 65 | ])) 66 | 67 | def _wait_for_response(self, command, timeout=None): 68 | timeout = Timeout(timeout or self._timeout) 69 | 70 | def read(size=1): 71 | self._serial.timeout = timeout.time_left() 72 | data = self._serial.read(size) 73 | if len(data) != size: 74 | raise TimeoutError() 75 | return data 76 | 77 | while True: 78 | data = [] 79 | data += read(1) 80 | if data[-1] != 0x55: 81 | LOGGER.error( 82 | 'Got unexpected octet while waiting for response header: %02x', 83 | data[-1] 84 | ) 85 | continue 86 | data += read(1) 87 | if data[-1] != 0x55: 88 | LOGGER.error( 89 | 'Got unexpected octet while waiting for response header: %02x', 90 | data[-1] 91 | ) 92 | continue 93 | data += read(2) 94 | length = data[2] 95 | cmd = data[3] 96 | 97 | data += read(length-2) if length > 2 else [] 98 | params = data[4:] 99 | 100 | LOGGER.debug('Got command %s response: %s', cmd, hex_data(data)) 101 | self._responses.append([cmd] + params) 102 | 103 | return params 104 | 105 | def _query(self, command, *params, timeout=None): 106 | with self._lock: 107 | self._command(command, *params) 108 | return self._wait_for_response(command, timeout=timeout) 109 | 110 | def move(self, positions, time=0): 111 | """Command multiple servos to move to given positions in given time. 112 | 113 | Args: 114 | positoins - dict mapping servo IDs to corresponding positions 115 | time - int number of milliseconds for move 116 | """ 117 | time = clamp(0, 30000, time) 118 | 119 | self._command( 120 | CMD_SERVO_MOVE, len(positions), 121 | lower_byte(time), higher_byte(time), 122 | *chain(*[ 123 | (servo_id, lower_byte(pos), higher_byte(pos)) 124 | for servo_id, position in positions.items() 125 | for pos in [clamp(0, 10000, position)] 126 | ]) 127 | ) 128 | 129 | def get_positions(self, servo_ids): 130 | """Reads positions of servos with given IDs and returns a map 131 | from servo ID to corresponding position. 132 | 133 | Args: 134 | servo_ids - list of servo IDs (ints) 135 | 136 | Returns: 137 | dict mapping servo ID to corresponding position 138 | """ 139 | response = self._query(CMD_MULT_SERVO_POS_READ, len(servo_ids), *servo_ids) 140 | return { 141 | response[1 + 3*i]: word(response[2 + 3*i], response[3 + 3*i]) 142 | for i in range(response[0]) 143 | } 144 | 145 | def unload(self, servo_ids): 146 | """Switches off motors of servos with given IDs. 147 | 148 | Args: 149 | servo_ids - list of servo IDs (ints) 150 | """ 151 | self._command(CMD_MULT_SERVO_UNLOAD, len(servo_ids), *servo_ids) 152 | 153 | def get_battery_voltage(self): 154 | """Returns servo controller power supply voltage level in millivolts.""" 155 | response = self._query(CMD_GET_BATTERY_VOLTAGE) 156 | return word(response[0], response[1]) 157 | -------------------------------------------------------------------------------- /lewansoul-lx16a/lewansoul_lx16a.py: -------------------------------------------------------------------------------- 1 | __all__ = [ 2 | 'ServoController', 3 | 'TimeoutError', 4 | 5 | 'SERVO_ERROR_OVER_TEMPERATURE', 6 | 'SERVO_ERROR_OVER_VOLTAGE', 7 | 'SERVO_ERROR_LOCKED_ROTOR', 8 | ] 9 | 10 | 11 | from serial.serialutil import Timeout 12 | from functools import partial 13 | import threading 14 | import logging 15 | 16 | 17 | SERVO_ID_ALL = 0xfe 18 | 19 | SERVO_MOVE_TIME_WRITE = 1 20 | SERVO_MOVE_TIME_READ = 2 21 | SERVO_MOVE_TIME_WAIT_WRITE = 7 22 | SERVO_MOVE_TIME_WAIT_READ = 8 23 | SERVO_MOVE_START = 11 24 | SERVO_MOVE_STOP = 12 25 | SERVO_ID_WRITE = 13 26 | SERVO_ID_READ = 14 27 | SERVO_ANGLE_OFFSET_ADJUST = 17 28 | SERVO_ANGLE_OFFSET_WRITE = 18 29 | SERVO_ANGLE_OFFSET_READ = 19 30 | SERVO_ANGLE_LIMIT_WRITE = 20 31 | SERVO_ANGLE_LIMIT_READ = 21 32 | SERVO_VIN_LIMIT_WRITE = 22 33 | SERVO_VIN_LIMIT_READ = 23 34 | SERVO_TEMP_MAX_LIMIT_WRITE = 24 35 | SERVO_TEMP_MAX_LIMIT_READ = 25 36 | SERVO_TEMP_READ = 26 37 | SERVO_VIN_READ = 27 38 | SERVO_POS_READ = 28 39 | SERVO_OR_MOTOR_MODE_WRITE = 29 40 | SERVO_OR_MOTOR_MODE_READ = 30 41 | SERVO_LOAD_OR_UNLOAD_WRITE = 31 42 | SERVO_LOAD_OR_UNLOAD_READ = 32 43 | SERVO_LED_CTRL_WRITE = 33 44 | SERVO_LED_CTRL_READ = 34 45 | SERVO_LED_ERROR_WRITE = 35 46 | SERVO_LED_ERROR_READ = 36 47 | 48 | 49 | SERVO_ERROR_OVER_TEMPERATURE = 1 50 | SERVO_ERROR_OVER_VOLTAGE = 2 51 | SERVO_ERROR_LOCKED_ROTOR = 4 52 | 53 | 54 | def lower_byte(value): 55 | return int(value) % 256 56 | 57 | 58 | def higher_byte(value): 59 | return int(value / 256) % 256 60 | 61 | 62 | def word(low, high): 63 | return int(low) + int(high)*256 64 | 65 | 66 | def clamp(range_min, range_max, value): 67 | return min(range_max, max(range_min, value)) 68 | 69 | 70 | class TimeoutError(RuntimeError): 71 | pass 72 | 73 | 74 | LOGGER = logging.getLogger('lewansoul.servos.lx16a') 75 | 76 | 77 | class Servo(object): 78 | def __init__(self, controller, servo_id): 79 | self.__dict__.update({ 80 | '_controller': controller, 81 | 'servo_id': servo_id, 82 | }) 83 | 84 | def __hasattr__(self, name): 85 | return hasattr(self._controller, name) 86 | 87 | def __getattr__(self, name): 88 | attr = getattr(self._controller, name) 89 | if callable(attr): 90 | attr = partial(attr, self.servo_id) 91 | return attr 92 | 93 | 94 | class ServoController(object): 95 | def __init__(self, serial, timeout=1): 96 | self._serial = serial 97 | self._timeout = timeout 98 | self._lock = threading.RLock() 99 | 100 | def _command(self, servo_id, command, *params): 101 | length = 3 + len(params) 102 | checksum = 255-((servo_id + length + command + sum(params)) % 256) 103 | LOGGER.debug('Sending servo control packet: %s', [ 104 | 0x55, 0x55, servo_id, length, command, *params, checksum 105 | ]) 106 | with self._lock: 107 | self._serial.write(bytearray([ 108 | 0x55, 0x55, servo_id, length, command, *params, checksum 109 | ])) 110 | 111 | def _wait_for_response(self, servo_id, command, timeout=None): 112 | timeout = Timeout(timeout or self._timeout) 113 | 114 | def read(size=1): 115 | self._serial.timeout = timeout.time_left() 116 | data = self._serial.read(size) 117 | if len(data) != size: 118 | raise TimeoutError() 119 | return data 120 | 121 | while True: 122 | data = [] 123 | data += read(1) 124 | if data[-1] != 0x55: 125 | continue 126 | data += read(1) 127 | if data[-1] != 0x55: 128 | continue 129 | data += read(3) 130 | sid = data[2] 131 | length = data[3] 132 | cmd = data[4] 133 | if length > 7: 134 | LOGGER.error('Invalid length for packet %s', list(data)) 135 | continue 136 | 137 | data += read(length-3) if length > 3 else [] 138 | params = data[5:] 139 | data += read(1) 140 | checksum = data[-1] 141 | if 255-(sid + length + cmd + sum(params)) % 256 != checksum: 142 | LOGGER.error('Invalid checksum for packet %s', list(data)) 143 | continue 144 | 145 | if cmd != command: 146 | LOGGER.warning('Got unexpected command %s response %s', 147 | cmd, list(data)) 148 | continue 149 | 150 | if servo_id != SERVO_ID_ALL and sid != servo_id: 151 | LOGGER.warning('Got command response from unexpected servo %s', sid) 152 | continue 153 | 154 | return [sid, cmd, *params] 155 | 156 | def _query(self, servo_id, command, timeout=None): 157 | with self._lock: 158 | self._command(servo_id, command) 159 | return self._wait_for_response(servo_id, command, timeout=timeout) 160 | 161 | def servo(self, servo_id): 162 | return Servo(self, servo_id) 163 | 164 | def get_servo_id(self, servo_id=SERVO_ID_ALL, timeout=None): 165 | response = self._query(servo_id, SERVO_ID_READ, timeout=timeout) 166 | return response[2] 167 | 168 | def set_servo_id(self, servo_id, new_servo_id): 169 | self._command(servo_id, SERVO_ID_WRITE, new_servo_id) 170 | 171 | def move(self, servo_id, position, time=0): 172 | position = clamp(0, 1000, position) 173 | time = clamp(0, 30000, time) 174 | 175 | self._command( 176 | servo_id, SERVO_MOVE_TIME_WRITE, 177 | lower_byte(position), higher_byte(position), 178 | lower_byte(time), higher_byte(time), 179 | ) 180 | 181 | def get_prepared_move(self, servo_id, timeout=None): 182 | """Returns servo position and time tuple""" 183 | response = self._query(servo_id, SERVO_MOVE_TIME_WAIT_READ, timeout=timeout) 184 | return word(response[2], response[3]), word(response[4], response[5]) 185 | 186 | def move_prepare(self, servo_id, position, time=0): 187 | position = clamp(0, 1000, position) 188 | time = clamp(0, 30000, time) 189 | 190 | self._command( 191 | servo_id, SERVO_MOVE_TIME_WAIT_WRITE, 192 | lower_byte(position), higher_byte(position), 193 | lower_byte(time), higher_byte(time), 194 | ) 195 | 196 | def move_start(self, servo_id=SERVO_ID_ALL): 197 | self._command(servo_id, SERVO_MOVE_START) 198 | 199 | def move_stop(self, servo_id=SERVO_ID_ALL): 200 | self._command(servo_id, SERVO_MOVE_STOP) 201 | 202 | def get_position_offset(self, servo_id, timeout=None): 203 | response = self._query(servo_id, SERVO_ANGLE_OFFSET_READ, timeout=timeout) 204 | deviation = response[2] 205 | if deviation > 127: 206 | deviation -= 256 207 | return deviation 208 | 209 | def set_position_offset(self, servo_id, deviation): 210 | deviation = clamp(-125, 125, deviation) 211 | if deviation < 0: 212 | deviation += 256 213 | self._command(servo_id, SERVO_ANGLE_OFFSET_ADJUST, deviation) 214 | 215 | def save_position_offset(self, servo_id): 216 | self._command(servo_id, SERVO_ANGLE_OFFSET_WRITE) 217 | 218 | def get_position_limits(self, servo_id, timeout=None): 219 | response = self._query(servo_id, SERVO_ANGLE_LIMIT_READ, timeout=timeout) 220 | return word(response[2], response[3]), word(response[4], response[5]) 221 | 222 | def set_position_limits(self, servo_id, min_position, max_position): 223 | min_position = clamp(0, 1000, min_position) 224 | max_position = clamp(0, 1000, max_position) 225 | self._command( 226 | servo_id, SERVO_ANGLE_LIMIT_WRITE, 227 | lower_byte(min_position), higher_byte(min_position), 228 | lower_byte(max_position), higher_byte(max_position), 229 | ) 230 | 231 | def get_voltage_limits(self, servo_id, timeout=None): 232 | response = self._query(servo_id, SERVO_VIN_LIMIT_READ, timeout=timeout) 233 | return word(response[2], response[3]), word(response[4], response[5]) 234 | 235 | def set_voltage_limits(self, servo_id, min_voltage, max_voltage): 236 | min_voltage = clamp(4500, 12000, min_voltage) 237 | max_voltage = clamp(4500, 12000, max_voltage) 238 | self._command( 239 | servo_id, SERVO_VIN_LIMIT_WRITE, 240 | lower_byte(min_voltage), higher_byte(min_voltage), 241 | lower_byte(max_voltage), higher_byte(max_voltage), 242 | ) 243 | 244 | def get_max_temperature_limit(self, servo_id, timeout=None): 245 | response = self._query(servo_id, SERVO_TEMP_MAX_LIMIT_READ, timeout=timeout) 246 | return response[2] 247 | 248 | def set_max_temperature_limit(self, servo_id, max_temperature): 249 | max_temperature = clamp(50, 100, max_temperature) 250 | self._command(servo_id, SERVO_TEMP_MAX_LIMIT_WRITE, max_temperature) 251 | 252 | def get_temperature(self, servo_id, timeout=None): 253 | response = self._query(servo_id, SERVO_TEMP_READ, timeout=timeout) 254 | return response[2] 255 | 256 | def get_voltage(self, servo_id, timeout=None): 257 | response = self._query(servo_id, SERVO_VIN_READ, timeout=timeout) 258 | return word(response[2], response[3]) 259 | 260 | def get_position(self, servo_id, timeout=None): 261 | response = self._query(servo_id, SERVO_POS_READ, timeout=timeout) 262 | position = word(response[2], response[3]) 263 | if position > 32767: 264 | position -= 65536 265 | return position 266 | 267 | def get_mode(self, servo_id, timeout=None): 268 | response = self._query(servo_id, SERVO_OR_MOTOR_MODE_READ, timeout=timeout) 269 | return response[2] 270 | 271 | def get_motor_speed(self, servo_id, timeout=None): 272 | response = self._query(servo_id, SERVO_OR_MOTOR_MODE_READ, timeout=timeout) 273 | if response[2] != 1: 274 | return 0 275 | speed = word(response[4], response[5]) 276 | if speed > 32767: 277 | speed -= 65536 278 | return speed 279 | 280 | def set_servo_mode(self, servo_id): 281 | self._command( 282 | servo_id, SERVO_OR_MOTOR_MODE_WRITE, 0, 0, 0, 0, 283 | ) 284 | 285 | def set_motor_mode(self, servo_id, speed=0): 286 | speed = clamp(-1000, 1000, speed) 287 | if speed < 0: 288 | speed += 65536 289 | self._command( 290 | servo_id, SERVO_OR_MOTOR_MODE_WRITE, 1, 0, 291 | lower_byte(speed), higher_byte(speed), 292 | ) 293 | 294 | def is_motor_on(self, servo_id, timeout=None): 295 | response = self._query(servo_id, SERVO_LOAD_OR_UNLOAD_READ, timeout=timeout) 296 | return response[2] == 1 297 | 298 | def motor_on(self, servo_id): 299 | self._command(servo_id, SERVO_LOAD_OR_UNLOAD_WRITE, 1) 300 | 301 | def motor_off(self, servo_id): 302 | self._command(servo_id, SERVO_LOAD_OR_UNLOAD_WRITE, 0) 303 | 304 | def is_led_on(self, servo_id, timeout=None): 305 | response = self._query(servo_id, SERVO_LED_CTRL_READ, timeout=timeout) 306 | return response[2] == 0 307 | 308 | def led_on(self, servo_id): 309 | self._command(servo_id, SERVO_LED_CTRL_WRITE, 0) 310 | 311 | def led_off(self, servo_id): 312 | self._command(servo_id, SERVO_LED_CTRL_WRITE, 1) 313 | 314 | def get_led_errors(self, servo_id, timeout=None): 315 | response = self._query(servo_id, SERVO_LED_ERROR_READ, timeout=timeout) 316 | return response[2] 317 | 318 | def set_led_errors(self, servo_id, error): 319 | error = clamp(0, 7, error) 320 | self._command(servo_id, SERVO_LED_ERROR_WRITE, error) 321 | -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/resources/ServoTerminal.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | ServoTerminal 4 | 5 | 6 | 7 | 0 8 | 0 9 | 800 10 | 478 11 | 12 | 13 | 14 | LewanSoul Servo Terminal 15 | 16 | 17 | 18 | 0 19 | 20 | 21 | 22 | 23 | 24 | 25 | Port 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 300 34 | 0 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | Refresh 43 | 44 | 45 | 46 | 47 | 48 | 49 | Qt::Horizontal 50 | 51 | 52 | 53 | 0 54 | 0 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 10 66 | 67 | 68 | 0 69 | 70 | 71 | 0 72 | 73 | 74 | 75 | 76 | 10 77 | 78 | 79 | 0 80 | 81 | 82 | 83 | 84 | Scan Servos 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 0 93 | 0 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 10 105 | 106 | 107 | 108 | 109 | Qt::Vertical 110 | 111 | 112 | QSizePolicy::Fixed 113 | 114 | 115 | 116 | 20 117 | 20 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | Temperature: 0 126 | 127 | 128 | 129 | 130 | 131 | 132 | Qt::Vertical 133 | 134 | 135 | 136 | 20 137 | 40 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | Configure 146 | 147 | 148 | 149 | 150 | 151 | 152 | Servo ID 153 | 154 | 155 | 156 | 157 | 158 | 159 | 0 .. 1000 160 | 161 | 162 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 163 | 164 | 165 | 166 | 167 | 168 | 169 | Position Limits 170 | 171 | 172 | 173 | 174 | 175 | 176 | Qt::Vertical 177 | 178 | 179 | QSizePolicy::Fixed 180 | 181 | 182 | 183 | 20 184 | 20 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | Voltage Limits 193 | 194 | 195 | 196 | 197 | 198 | 199 | Voltage: 0 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 0 210 | 0 211 | 212 | 213 | 214 | Servo 215 | 216 | 217 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 100 226 | 0 227 | 228 | 229 | 230 | 231 | 100 232 | 16777215 233 | 234 | 235 | 236 | 1 237 | 238 | 239 | 1 240 | 241 | 242 | Qt::Horizontal 243 | 244 | 245 | QSlider::TicksBelow 246 | 247 | 248 | 249 | 250 | 251 | 252 | Motor 253 | 254 | 255 | 256 | 257 | 258 | 259 | Qt::Horizontal 260 | 261 | 262 | 263 | 40 264 | 20 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | Max Temperature 275 | 276 | 277 | 278 | 279 | 280 | 281 | 1 282 | 283 | 284 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 285 | 286 | 287 | 288 | 289 | 290 | 291 | Configure 292 | 293 | 294 | 295 | 296 | 297 | 298 | Motor On 299 | 300 | 301 | true 302 | 303 | 304 | false 305 | 306 | 307 | 308 | 309 | 310 | 311 | 6500 .. 12000 312 | 313 | 314 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 315 | 316 | 317 | 318 | 319 | 320 | 321 | Configure 322 | 323 | 324 | 325 | 326 | 327 | 328 | Change ID 329 | 330 | 331 | 332 | 333 | 334 | 335 | 85 336 | 337 | 338 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 339 | 340 | 341 | 342 | 343 | 344 | 345 | Led On 346 | 347 | 348 | true 349 | 350 | 351 | 352 | 353 | 354 | Clear Errors 355 | false 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 0 365 | 366 | 367 | 368 | 369 | 0 370 | 371 | 372 | 0 373 | 374 | 375 | 0 376 | 377 | 378 | 0 379 | 380 | 381 | 0 382 | 383 | 384 | 10 385 | 386 | 387 | 388 | 389 | Position: 390 | 391 | 392 | 393 | 394 | 395 | 396 | 0 397 | 398 | 399 | 400 | 401 | 402 | 403 | Set Position: 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 0 413 | 414 | 415 | 1000 416 | 417 | 418 | 0 419 | 420 | 421 | Qt::Horizontal 422 | 423 | 424 | QSlider::TicksBelow 425 | 426 | 427 | 100 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 0 436 | 0 437 | 438 | 439 | 440 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 441 | 442 | 443 | 1000 444 | 445 | 446 | false 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | Position Offset: 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | Configure 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 0 473 | 474 | 475 | 0 476 | 477 | 478 | 0 479 | 480 | 481 | 0 482 | 483 | 484 | 0 485 | 486 | 487 | 10 488 | 489 | 490 | 491 | 492 | Speed: 493 | 494 | 495 | 496 | 497 | 498 | 499 | 0 500 | 501 | 502 | 503 | 504 | 505 | 506 | Set Speed: 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | -1000 516 | 517 | 518 | 1000 519 | 520 | 521 | 0 522 | 523 | 524 | Qt::Horizontal 525 | 526 | 527 | QSlider::TicksBelow 528 | 529 | 530 | 100 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 0 539 | 0 540 | 541 | 542 | 543 | Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter 544 | 545 | 546 | -1000 547 | 548 | 549 | 1000 550 | 551 | 552 | false 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 572 | 573 | -------------------------------------------------------------------------------- /lewansoul-lx16a-terminal/lewansoul_lx16a_terminal.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """ 3 | Terminal GUI for LewanSoul LX-16A servos 4 | 5 | Author: Maxim Kulkin 6 | Website: https://github.com/maximkulkin/lewansoul-lx16a 7 | """ 8 | 9 | import sys 10 | 11 | from PyQt5.QtCore import Qt, QTimer, QThread, pyqtSignal 12 | from PyQt5.QtWidgets import (QWidget, QApplication, QDialog, QMessageBox, QListWidgetItem) 13 | from PyQt5.uic import loadUi as _loadUi 14 | 15 | from collections import namedtuple 16 | import pkg_resources 17 | import logging 18 | import serial 19 | from serial.tools.list_ports import comports 20 | import lewansoul_lx16a 21 | 22 | 23 | def loadUi(path, widget): 24 | real_path = pkg_resources.resource_filename('lewansoul_lx16a_terminal', path) 25 | _loadUi(real_path, widget) 26 | 27 | 28 | ServoConfiguration = namedtuple('ServoConfiguration', [ 29 | 'servo_id', 'position_limits', 'voltage_limits', 'max_temperature', 'position_offset', 30 | ]) 31 | 32 | ServoState = namedtuple('ServoState', [ 33 | 'servo_id', 'voltage', 'temperature', 34 | 'mode', 'position', 'speed', 'motor_on', 'led_on', 'led_errors', 35 | ]) 36 | 37 | 38 | class ConfigureIdDialog(QDialog): 39 | def __init__(self): 40 | super(ConfigureIdDialog, self).__init__() 41 | 42 | loadUi('resources/ConfigureId.ui', self) 43 | 44 | self.okButton.clicked.connect(self.accept) 45 | self.cancelButton.clicked.connect(self.reject) 46 | 47 | @property 48 | def servoId(self): 49 | return self.servoIdEdit.value() 50 | 51 | @servoId.setter 52 | def servoId(self, value): 53 | self.servoIdEdit.setValue(value) 54 | 55 | 56 | class ConfigurePositionLimitsDialog(QDialog): 57 | def __init__(self): 58 | super(ConfigurePositionLimitsDialog, self).__init__() 59 | 60 | loadUi('resources/ConfigurePositionLimits.ui', self) 61 | 62 | self.okButton.clicked.connect(self.accept) 63 | self.cancelButton.clicked.connect(self.reject) 64 | 65 | @property 66 | def minPosition(self): 67 | return self.rangeMin.value() 68 | 69 | @minPosition.setter 70 | def minPosition(self, value): 71 | self.rangeMin.setValue(value) 72 | 73 | @property 74 | def maxPosition(self): 75 | return self.rangeMax.value() 76 | 77 | @maxPosition.setter 78 | def maxPosition(self, value): 79 | self.rangeMax.setValue(value) 80 | 81 | 82 | class ConfigureVoltageLimitsDialog(QDialog): 83 | def __init__(self): 84 | super(ConfigureVoltageLimitsDialog, self).__init__() 85 | 86 | loadUi('resources/ConfigureVoltageLimits.ui', self) 87 | 88 | self.okButton.clicked.connect(self.accept) 89 | self.cancelButton.clicked.connect(self.reject) 90 | 91 | @property 92 | def minVoltage(self): 93 | return self.rangeMin.value() 94 | 95 | @minVoltage.setter 96 | def minVoltage(self, value): 97 | self.rangeMin.setValue(value) 98 | 99 | @property 100 | def maxVoltage(self): 101 | return self.rangeMax.value() 102 | 103 | @maxVoltage.setter 104 | def maxVoltage(self, value): 105 | self.rangeMax.setValue(value) 106 | 107 | 108 | class ConfigureMaxTemperatureDialog(QDialog): 109 | def __init__(self): 110 | super(ConfigureMaxTemperatureDialog, self).__init__() 111 | 112 | loadUi('resources/ConfigureMaxTemperature.ui', self) 113 | 114 | self.okButton.clicked.connect(self.accept) 115 | self.cancelButton.clicked.connect(self.reject) 116 | 117 | @property 118 | def maxTemperature(self): 119 | return self.maxTemperatureEdit.value() 120 | 121 | @maxTemperature.setter 122 | def maxTemperature(self, value): 123 | self.maxTemperatureEdit.setValue(value) 124 | 125 | 126 | class ConfigurePositionOffsetDialog(QDialog): 127 | def __init__(self, servo): 128 | super(ConfigurePositionOffsetDialog, self).__init__() 129 | self._servo = servo 130 | 131 | loadUi('resources/ConfigurePositionOffset.ui', self) 132 | 133 | self.positionOffsetSlider.valueChanged.connect(self._on_slider_change) 134 | self.positionOffsetEdit.valueChanged.connect(self._on_edit_change) 135 | 136 | self.okButton.clicked.connect(self.accept) 137 | self.cancelButton.clicked.connect(self.reject) 138 | 139 | def _update_servo(self, deviation): 140 | self._servo.set_position_offset(deviation) 141 | 142 | def _on_slider_change(self, position): 143 | self.positionOffsetEdit.setValue(position) 144 | self._update_servo(position) 145 | 146 | def _on_edit_change(self, position): 147 | self.positionOffsetSlider.setValue(position) 148 | self._update_servo(position) 149 | 150 | @property 151 | def positionOffset(self): 152 | return self.positionOffsetEdit.value() 153 | 154 | @positionOffset.setter 155 | def positionOffset(self, value): 156 | self.positionOffsetEdit.setValue(value) 157 | 158 | 159 | class ServoScanThread(QThread): 160 | servoFound = pyqtSignal(int) 161 | 162 | def __init__(self, controller): 163 | super(ServoScanThread, self).__init__() 164 | self._controller = controller 165 | 166 | def _servo_exists(self, id): 167 | try: 168 | servo_id = self._controller.get_servo_id(id, timeout=0.2) 169 | return (servo_id == id) 170 | except TimeoutError: 171 | return False 172 | 173 | def run(self): 174 | for servoId in range(1, 253): 175 | if self.isInterruptionRequested(): 176 | break 177 | 178 | try: 179 | if self._servo_exists(servoId): 180 | self.servoFound.emit(servoId) 181 | except lewansoul_lx16a.TimeoutError: 182 | pass 183 | 184 | self.yieldCurrentThread() 185 | 186 | 187 | class GetServoConfigurationThread(QThread): 188 | servoConfigurationUpdated = pyqtSignal(ServoConfiguration) 189 | servoConfigurationTimeout = pyqtSignal() 190 | 191 | MAX_RETRIES = 5 192 | 193 | def __init__(self, servo): 194 | super(GetServoConfigurationThread, self).__init__() 195 | self._servo = servo 196 | 197 | @property 198 | def servo_id(self): 199 | return self._servo.servo_id 200 | 201 | def run(self): 202 | try: 203 | retries = 0 204 | while True: 205 | if self.isInterruptionRequested(): 206 | return 207 | 208 | try: 209 | position_limits = self._servo.get_position_limits() 210 | break 211 | except lewansoul_lx16a.TimeoutError: 212 | retries += 1 213 | if retries >= self.MAX_RETRIES: 214 | raise 215 | self.sleep(1) 216 | 217 | while True: 218 | if self.isInterruptionRequested(): 219 | return 220 | 221 | try: 222 | voltage_limits = self._servo.get_voltage_limits() 223 | break 224 | except lewansoul_lx16a.TimeoutError: 225 | retries += 1 226 | if retries >= self.MAX_RETRIES: 227 | raise 228 | self.sleep(1) 229 | 230 | while True: 231 | if self.isInterruptionRequested(): 232 | return 233 | 234 | try: 235 | max_temperature = self._servo.get_max_temperature_limit() 236 | break 237 | except lewansoul_lx16a.TimeoutError: 238 | retries += 1 239 | if retries >= self.MAX_RETRIES: 240 | raise 241 | self.sleep(1) 242 | 243 | while True: 244 | if self.isInterruptionRequested(): 245 | return 246 | 247 | try: 248 | position_offset = self._servo.get_position_offset() 249 | break 250 | except lewansoul_lx16a.TimeoutError: 251 | retries += 1 252 | if retries >= self.MAX_RETRIES: 253 | raise 254 | self.sleep(1) 255 | 256 | self.servoConfigurationUpdated.emit(ServoConfiguration( 257 | servo_id=self.servo_id, 258 | position_limits=position_limits, 259 | voltage_limits=voltage_limits, 260 | max_temperature=max_temperature, 261 | position_offset=position_offset, 262 | )) 263 | except lewansoul_lx16a.TimeoutError: 264 | self.servoConfigurationTimeout.emit() 265 | 266 | 267 | class ServoMonitorThread(QThread): 268 | servoStateUpdated = pyqtSignal(ServoState) 269 | 270 | def __init__(self, servo): 271 | super(ServoMonitorThread, self).__init__() 272 | self._servo = servo 273 | 274 | def run(self): 275 | while not self.isInterruptionRequested(): 276 | try: 277 | voltage = self._servo.get_voltage() 278 | temperature = self._servo.get_temperature() 279 | 280 | mode = self._servo.get_mode() 281 | if mode == 0: 282 | position, speed = self._servo.get_position(), 0 283 | else: 284 | position, speed = 0, self._servo.get_motor_speed() 285 | 286 | motor_on = self._servo.is_motor_on() 287 | led_on = self._servo.is_led_on() 288 | led_errors = self._servo.get_led_errors() 289 | 290 | self.servoStateUpdated.emit(ServoState( 291 | servo_id=self._servo.servo_id, 292 | voltage=voltage, 293 | temperature=temperature, 294 | mode=mode, 295 | position=position, 296 | speed=speed, 297 | motor_on=motor_on, 298 | led_on=led_on, 299 | led_errors=led_errors, 300 | )) 301 | except lewansoul_lx16a.TimeoutError: 302 | pass 303 | 304 | self.sleep(1) 305 | 306 | 307 | class Terminal(QWidget): 308 | logger = logging.getLogger('lewansoul.terminal') 309 | 310 | def __init__(self): 311 | super(Terminal, self).__init__() 312 | 313 | self.connection = False 314 | self.selected_servo_id = False 315 | self.servo = None 316 | self._servo_initialization = False 317 | 318 | self._available_ports = [] 319 | 320 | loadUi('resources/ServoTerminal.ui', self) 321 | 322 | self.configureIdButton.clicked.connect(self._configure_servo_id) 323 | self.configurePositionLimitsButton.clicked.connect(self._configure_position_limits) 324 | self.configureVoltageLimitsButton.clicked.connect(self._configure_voltage_limits) 325 | self.configureMaxTemperatureButton.clicked.connect(self._configure_max_temperature) 326 | self.configurePositionOffsetButton.clicked.connect(self._configure_position_offset) 327 | 328 | self.portCombo.currentTextChanged.connect(self._on_port_change) 329 | self.refreshPortsButton.clicked.connect(self._refresh_ports) 330 | self.servoList.currentItemChanged.connect(lambda curItem, prevItem: self._on_servo_selected(curItem)) 331 | self.scanServosButton.clicked.connect(self._scan_servos) 332 | 333 | self.servoOrMotorSwitch.valueChanged.connect(self._on_servo_motor_switch) 334 | self.speedSlider.valueChanged.connect(self._on_speed_slider_change) 335 | self.speedEdit.valueChanged.connect(self._on_speed_edit_change) 336 | 337 | self.positionSlider.valueChanged.connect(self._on_position_slider_change) 338 | self.positionEdit.valueChanged.connect(self._on_position_edit_change) 339 | 340 | self.motorOnButton.clicked.connect(self._on_motor_on_button) 341 | self.ledOnButton.clicked.connect(self._on_led_on_button) 342 | self.clearLedErrorsButton.clicked.connect(self._on_clear_led_errors_button) 343 | 344 | self._servoScanThread = None 345 | self._servoReadConfigurationThread = None 346 | self._servoStateMonitorThread = None 347 | 348 | self.connectionGroup.setEnabled(False) 349 | 350 | self._refresh_ports() 351 | self._on_servo_selected(None) 352 | 353 | self.show() 354 | 355 | def _refresh_ports(self): 356 | old_text = self.portCombo.currentText() 357 | 358 | self._available_ports = sorted(comports()) 359 | 360 | self.portCombo.blockSignals(True) 361 | self.portCombo.clear() 362 | self.portCombo.addItem('') 363 | self.portCombo.addItems([port.device for port in self._available_ports]) 364 | self.portCombo.setCurrentText(old_text) 365 | self.portCombo.blockSignals(False) 366 | 367 | if self.portCombo.currentText() != old_text: 368 | self._on_port_change(self.portCombo.currentIndex()) 369 | 370 | def _on_port_change(self, portIdx): 371 | self._connect_to_port(self.portCombo.currentText()) 372 | 373 | def _connect_to_port(self, device): 374 | if self.connection: 375 | self.connection.close() 376 | self.logger.info('Disconnected') 377 | self.connection = None 378 | self.controller = None 379 | self.connectionGroup.setEnabled(False) 380 | self.servoList.clear() 381 | self._on_servo_selected(None) 382 | 383 | if device: 384 | self.logger.info('Connecting to port %s' % device) 385 | try: 386 | self.connection = serial.Serial(device, 115200, timeout=1) 387 | self.controller = lewansoul_lx16a.ServoController(self.connection, timeout=1) 388 | self.connectionGroup.setEnabled(True) 389 | self.logger.info('Connected to {}'.format(device)) 390 | except serial.serialutil.SerialException as e: 391 | self.logger.error('Failed to connect to port {}'.format(device)) 392 | QMessageBox.critical(self, "Connection error", "Failed to connect to device") 393 | 394 | def _scan_servos(self): 395 | if not self.controller: 396 | return 397 | 398 | def scanStarted(): 399 | self.servoList.clear() 400 | self.scanServosButton.setText('Stop Scan') 401 | self.scanServosButton.setEnabled(True) 402 | 403 | def scanFinished(): 404 | self.scanServosButton.setText('Scan Servos') 405 | self.scanServosButton.setEnabled(True) 406 | self._servoScanThread = None 407 | 408 | def servoFound(servoId): 409 | item = QListWidgetItem('Servo ID=%s' % servoId) 410 | item.setData(Qt.UserRole, servoId) 411 | self.servoList.addItem(item) 412 | 413 | if not self._servoScanThread: 414 | self._servoScanThread = ServoScanThread(self.controller) 415 | self._servoScanThread.servoFound.connect(servoFound) 416 | self._servoScanThread.started.connect(scanStarted) 417 | self._servoScanThread.finished.connect(scanFinished) 418 | 419 | self.scanServosButton.setEnabled(False) 420 | if self._servoScanThread.isRunning(): 421 | self._servoScanThread.requestInterruption() 422 | else: 423 | self._servoScanThread.start() 424 | 425 | def _on_servo_selected(self, item): 426 | servo_id = None 427 | if item is not None: 428 | servo_id = item.data(Qt.UserRole) 429 | 430 | if servo_id: 431 | self.servo = self.controller.servo(servo_id) 432 | self.servoGroup.setEnabled(True) 433 | 434 | def servo_configuration_updated(details): 435 | self.servoIdLabel.setText(str(details.servo_id)) 436 | self.positionLimits.setText('%d .. %d' % details.position_limits) 437 | self.voltageLimits.setText('%d .. %d' % details.voltage_limits) 438 | self.maxTemperature.setText(str(details.max_temperature)) 439 | self.positionOffset.setText(str(details.position_offset)) 440 | self._servo_initialization = True 441 | 442 | if self._servoStateMonitorThread: 443 | self._servoStateMonitorThread.requestInterruption() 444 | 445 | self._servoStateMonitorThread = ServoMonitorThread(self.servo) 446 | self._servoStateMonitorThread.servoStateUpdated.connect(self._update_servo_state) 447 | self._servoStateMonitorThread.start() 448 | 449 | def servo_configuration_timeout(): 450 | self._on_servo_selected(None) 451 | QMessageBox.warning(self, "Timeout", "Timeout reading servo data") 452 | 453 | if self._servoReadConfigurationThread and self._servoReadConfigurationThread.servo_id != servo_id: 454 | self._servoReadConfigurationThread.requestInterruption() 455 | self._servoReadConfigurationThread.wait() 456 | self._servoReadConfigurationThread = None 457 | 458 | if self._servoReadConfigurationThread is None: 459 | self._servoReadConfigurationThread = GetServoConfigurationThread(self.servo) 460 | self._servoReadConfigurationThread.servoConfigurationUpdated.connect(servo_configuration_updated) 461 | self._servoReadConfigurationThread.servoConfigurationTimeout.connect(servo_configuration_timeout) 462 | self._servoReadConfigurationThread.start() 463 | else: 464 | if self._servoStateMonitorThread: 465 | self._servoStateMonitorThread.requestInterruption() 466 | self._servoStateMonitorThread.wait() 467 | self._servoStateMonitorThread = None 468 | 469 | if self._servoReadConfigurationThread: 470 | self._servoReadConfigurationThread.requestInterruption() 471 | self._servoReadConfigurationThread.wait() 472 | self._servoReadConfigurationThread = None 473 | 474 | self.servo = None 475 | 476 | self.servoIdLabel.setText('') 477 | self.positionLimits.setText('') 478 | self.voltageLimits.setText('') 479 | self.maxTemperature.setText('') 480 | 481 | self.currentVoltage.setText('Voltage:') 482 | self.currentTemperature.setText('Temperature:') 483 | self.currentPosition.setText('') 484 | 485 | self.servoGroup.setEnabled(False) 486 | 487 | def _configure_servo_id(self): 488 | if not self.servo: 489 | return 490 | 491 | dialog = ConfigureIdDialog() 492 | dialog.servoId = self.servo.get_servo_id() 493 | if dialog.exec_(): 494 | self.logger.info('Setting servo ID to %d' % dialog.servoId) 495 | self.servo.set_servo_id(dialog.servoId) 496 | self.servo = self.controller.servo(dialog.servoId) 497 | self.servoIdLabel.setText(str(dialog.servoId)) 498 | item = self.servoList.currentItem() 499 | if item is not None: 500 | item.setText('Servo ID=%d' % dialog.servoId) 501 | item.setData(Qt.UserRole, dialog.servoId) 502 | 503 | def _configure_position_limits(self): 504 | if not self.servo: 505 | return 506 | 507 | dialog = ConfigurePositionLimitsDialog() 508 | dialog.minPosition, dialog.maxPosition = self.servo.get_position_limits() 509 | if dialog.exec_(): 510 | self.logger.info('Setting position limits to %d..%d' % (dialog.minPosition, dialog.maxPosition)) 511 | self.servo.set_position_limits(dialog.minPosition, dialog.maxPosition) 512 | self.positionLimits.setText('%d .. %d' % (dialog.minPosition, dialog.maxPosition)) 513 | 514 | def _configure_voltage_limits(self): 515 | if not self.servo: 516 | return 517 | 518 | dialog = ConfigureVoltageLimitsDialog() 519 | dialog.minVoltage, dialog.maxVoltage = self.servo.get_voltage_limits() 520 | if dialog.exec_(): 521 | self.logger.info('Setting voltage limits to %d..%d' % (dialog.minVoltage, dialog.maxVoltage)) 522 | self.servo.set_voltage_limits(dialog.minVoltage, dialog.maxVoltage) 523 | self.voltageLimits.setText('%d .. %d' % (dialog.minVoltage, dialog.maxVoltage)) 524 | 525 | def _configure_max_temperature(self): 526 | if not self.servo: 527 | return 528 | 529 | dialog = ConfigureMaxTemperatureDialog() 530 | dialog.maxTemperature = self.servo.get_max_temperature_limit() 531 | if dialog.exec_(): 532 | self.logger.info('Setting max temperature limit to %d' % (dialog.maxTemperature)) 533 | self.servo.set_max_temperature_limit(dialog.maxTemperature) 534 | self.maxTemperature.setText(str(dialog.maxTemperature)) 535 | 536 | def _configure_position_offset(self): 537 | if not self.servo: 538 | return 539 | 540 | dialog = ConfigurePositionOffsetDialog(self.servo) 541 | old_position_offset = self.servo.get_position_offset() 542 | dialog.positionOffset = old_position_offset 543 | if dialog.exec_(): 544 | self.logger.info('Setting position offset limit to %d' % (dialog.positionOffset)) 545 | self.servo.set_position_offset(dialog.positionOffset) 546 | self.servo.save_position_offset() 547 | self.positionOffset.setText(str(dialog.positionOffset)) 548 | else: 549 | self.servo.set_position_offset(old_position_offset) 550 | 551 | def _on_servo_motor_switch(self, value): 552 | try: 553 | if value == 0: 554 | self.servoOrMotorModeUi.setCurrentIndex(0) 555 | self.servo.set_servo_mode() 556 | else: 557 | self.servoOrMotorModeUi.setCurrentIndex(1) 558 | self.servo.set_motor_mode() 559 | except lewansoul_lx16a.TimeoutError: 560 | QMessageBox.critical(self, "Timeout", "Timeout changing motor mode") 561 | 562 | def _on_speed_slider_change(self, speed): 563 | try: 564 | self.speedEdit.setValue(speed) 565 | self.logger.info('Setting motor speed to %d' % speed) 566 | self.servo.set_motor_mode(speed) 567 | except lewansoul_lx16a.TimeoutError: 568 | QMessageBox.critical(self, "Timeout", "Timeout updating motor speed") 569 | 570 | def _on_speed_edit_change(self, speed): 571 | try: 572 | self.speedSlider.setValue(speed) 573 | self.logger.info('Setting motor speed to %d' % speed) 574 | self.servo.set_motor_mode(speed) 575 | except lewansoul_lx16a.TimeoutError: 576 | QMessageBox.critical(self, "Timeout", "Timeout updating motor speed") 577 | 578 | def _on_position_slider_change(self, position): 579 | try: 580 | self.positionEdit.setValue(position) 581 | self.logger.info('Setting servo position to %d' % position) 582 | self.servo.move(position) 583 | except lewansoul_lx16a.TimeoutError: 584 | QMessageBox.critical(self, "Timeout", "Timeout setting servo position") 585 | 586 | def _on_position_edit_change(self, position): 587 | try: 588 | self.positionSlider.setValue(position) 589 | self.logger.info('Setting servo position to %d' % position) 590 | self.servo.move(position) 591 | except lewansoul_lx16a.TimeoutError: 592 | QMessageBox.critical(self, "Timeout", "Timeout setting servo position") 593 | 594 | def _on_motor_on_button(self): 595 | if not self.servo: 596 | return 597 | 598 | try: 599 | if self.servo.is_motor_on(): 600 | if self.servo.get_mode() == 0: 601 | self.servo.motor_off() 602 | else: 603 | self.servo.set_motor_mode(0) 604 | else: 605 | if self.servo.get_mode() == 0: 606 | self.servo.motor_on() 607 | else: 608 | self.servo.set_motor_mode(self.speedSlider.value()) 609 | except lewansoul_lx16a.TimeoutError: 610 | QMessageBox.critical(self, "Timeout", "Timeout updating motor state") 611 | 612 | def _on_led_on_button(self): 613 | if not self.servo: 614 | return 615 | 616 | try: 617 | if self.servo.is_led_on(): 618 | self.servo.led_off() 619 | self.ledOnButton.setChecked(False) 620 | else: 621 | self.servo.led_on() 622 | self.ledOnButton.setChecked(True) 623 | except lewansoul_lx16a.TimeoutError: 624 | QMessageBox.critical(self, "Timeout", "Timeout updating LED state") 625 | 626 | def _on_clear_led_errors_button(self): 627 | if not self.servo: 628 | return 629 | 630 | try: 631 | self.servo.set_led_errors(0) 632 | except lewansoul_lx16a.TimeoutError: 633 | QMessageBox.critical(self, "Timeout", "Timeout resetting LED errors") 634 | 635 | def _update_servo_state(self, servo_state): 636 | self.currentVoltage.setText('Voltage: %d' % servo_state.voltage) 637 | self.currentTemperature.setText('Temperature: %d' % servo_state.temperature) 638 | self.motorOnButton.setChecked(servo_state.motor_on) 639 | self.ledOnButton.setChecked(servo_state.led_on) 640 | if servo_state.led_errors: 641 | messages = [] 642 | if lewansoul_lx16a.SERVO_ERROR_OVER_TEMPERATURE | servo_state.led_errors: 643 | messages.append('Overheating') 644 | if lewansoul_lx16a.SERVO_ERROR_OVER_VOLTAGE | servo_state.led_errors: 645 | messages.append('Voltage is out of limits') 646 | if lewansoul_lx16a.SERVO_ERROR_LOCKED_ROTOR | servo_state.led_errors: 647 | messages.append('Locked rotor') 648 | self.ledErrors.setText('\n'.join(messages)) 649 | self.clearLedErrorsButton.setEnabled(True) 650 | else: 651 | self.ledErrors.setText('') 652 | self.clearLedErrorsButton.setEnabled(False) 653 | 654 | if self.servoOrMotorModeUi.currentIndex() == 0: 655 | self.currentPosition.setText(str(servo_state.position)) 656 | if self._servo_initialization: 657 | self.positionSlider.setValue(servo_state.position) 658 | else: 659 | self.currentSpeed.setText(str(servo_state.speed)) 660 | if self._servo_initialization: 661 | self.speedSlider.setValue(servo_state.speed) 662 | 663 | self._servo_initialization = False 664 | 665 | def closeEvent(self, event): 666 | if self._servoScanThread: 667 | self._servoScanThread.requestInterruption() 668 | self._servoScanThread.wait() 669 | self._servoScanThread = None 670 | 671 | if self._servoReadConfigurationThread: 672 | self._servoReadConfigurationThread.requestInterruption() 673 | self._servoReadConfigurationThread.wait() 674 | self._servoReadConfigurationThread = None 675 | 676 | if self._servoStateMonitorThread: 677 | self._servoStateMonitorThread.requestInterruption() 678 | self._servoStateMonitorThread.wait() 679 | self._servoStateMonitorThread = None 680 | 681 | if self.connection: 682 | self.connection.close() 683 | 684 | event.accept() 685 | 686 | 687 | def main(): 688 | logging.basicConfig(level=logging.DEBUG) 689 | app = QApplication(sys.argv) 690 | terminal = Terminal() 691 | sys.exit(app.exec_()) 692 | 693 | 694 | if __name__ == '__main__': 695 | main() 696 | --------------------------------------------------------------------------------