def _connect_to_port(self, device):
if self.connection:
self.connection.close()
self.logger.info('Disconnected')
self.connection = None
self.controller = None
self.connectionGroup.setEnabled(False)
self.servoList.clear()
self._on_servo_selected(None)
if device:
self.logger.info('Connecting to port %s' % device)
try:
self.connection = serial.Serial(device, 115200, timeout=1)
self.controller = lewansoul_lx16a.ServoController(self.connection, timeout=1)
self.connectionGroup.setEnabled(True)
self.logger.info('Connected to {}'.format(device))
except serial.serialutil.SerialException as e:
self.logger.error('Failed to connect to port {}'.format(device))
QMessageBox.critical(self, "Connection error", "Failed to connect to device")
import sys
import serial
import lewansoul_lx16a
from time import sleep
SERIAL_PORT = '/dev/serial0'
controller = lewansoul_lx16a.ServoController(serial.Serial(SERIAL_PORT,
115200,
timeout=0.5),
timeout=1)
new_id = 42
servo_id = None
servo = None
if __name__ == '__main__':
if len(sys.argv) == 3:
target_id = int(sys.argv[1])
new_id = int(sys.argv[2])
else:
target_id = 254
new_id = int(sys.argv[1])
print("Searching for servo...", end='', flush=True)
servo_id = controller.get_servo_id(target_id)
if servo_id is None:
print("No servo found.")
quit()
rospy.init_node('hardware_handler')
#-------------------------------------------
rospy.loginfo("setting ports to low latency mode")
for tty in tty_list:
os.system('setserial ' + tty + ' low_latency')
#-------------------------------------------
rospy.loginfo("creating controller objects")
controller = [0] * 4
for i, tty in enumerate(tty_list):
controller[i] = lewansoul_lx16a.ServoController(
serial.Serial(tty, 115200, timeout=1))
#-------------------------------------------
rospy.loginfo("indexing servos")
legs = []
for i in range(6):
legs.append(leg(i))
for leg_num in range(6):
for servo_num in range(3):
for tty_num, tty in enumerate(tty_list):
id = (leg_num * 3) + servo_num + 1
try:
controller[tty_num].get_servo_id(id, timeout=0.05)
