对应的硬件为幻尔总线舵机测试板BusLinker V2.5
本人制作的该控制板的开源修缮版本：幻尔总线舵机测试板BusLinker V2.5 - 立创开源硬件平台
不到10元的成本，设计比官方的好得多。经过测试，驱动4个舵机且温度比官方的更低的多多
以下代码全部都是在树莓派5上面运行，请注意对应的串口配置是否正确，接线是否正确（TX、RX怎么着对调着试两下就行了）
一、简易控制总线舵机运动代码
1.1树莓派5的简易控制运动代码
import time
import serial
import struct
# 从include.h移植的宏定义
def GET_LOW_BYTE(A):
return (A) & 0xFF
def GET_HIGH_BYTE(A):
return ((A) >> 8) & 0xFF
def BYTE_TO_HW(A, B):
return ((A << 8) | B)
# 常量定义
ID_ALL = 254
LOBOT_SERVO_FRAME_HEADER = 0x55
LOBOT_SERVO_MOVE_TIME_WRITE = 1
# 校验和函数（从SerialServo.h移植）
def LobotCheckSum(buf):
temp = 0
for i in range(2, buf[3] + 2):
temp += buf[i]
temp = ~temp
return temp & 0xFF
# 舵机移动函数（从SerialServo.h移植）
def LobotSerialServoMove(ser, servo_id, position, move_time):
# 限制位置范围
if position < 0:
position = 0
if position > 1000:
position = 1000
# 构造数据包
buf = [0] * 10
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 7
buf[4] = LOBOT_SERVO_MOVE_TIME_WRITE
buf[5] = GET_LOW_BYTE(position)
buf[6] = GET_HIGH_BYTE(position)
buf[7] = GET_LOW_BYTE(move_time)
buf[8] = GET_HIGH_BYTE(move_time)
buf[9] = LobotCheckSum(buf)
# 发送数据
ser.write(bytearray(buf))
# 主程序（对应Arduino的setup和loop）
def main():
# setup部分 - 初始化串口
try:
# 打开串口，对应Serial.begin(115200)
ser = serial.Serial(
port='/dev/ttyAMA0', # 树莓派上可能是/dev/ttyAMA0或/dev/ttyS0
baudrate=115200,
timeout=1.0
)
# 对应delay(1000)
time.sleep(1)
print("舵机控制程序开始运行...")
print("按Ctrl+C停止")
try:
# loop部分 - 循环执行
while True:
# 控制舵机转到位置0，用时1000ms
LobotSerialServoMove(ser, 1, 0, 1000)
time.sleep(3.0)
# 控制舵机转到位置500，用时1舵机转到位置 1000")
LobotSerialServoMove(ser, 1, 500, 1000)
time.sleep(3.0)
except KeyboardInterrupt:
print("\n程序被用户中断")
except serial.SerialException as e:
print(f"串口错误: {e}")
print("请检查:")
print("1. 串口设备路径是否正确")
print("2. 舵机是否已连接")
print("3. 树莓派上可能需要: sudo chmod 666 /dev/ttyAMA0")
finally:
if 'ser' in locals() and ser.is_open:
ser.close()
print("串口已关闭")
if __name__ == "__main__":
main()1.2 16进制简易测试
你也可以选择直接发送16进制进行简易测试，波特率 115200
控制1号舵机到0位置 时间1000
0x55, 0x55, 0x01, 0x07, 0x01, 0x00, 0x00, 0xE8, 0x03, 0x0B控制1号舵机到500位置 时间1000
0x55, 0x55, 0x01, 0x07, 0x01, 0xF4, 0x01, 0xE8, 0x03, 0x16
二、总线舵机完整功能控制代码
2.1 库函数serial_servo.py
import time
import serial
import struct
# 字节操作宏函数
def GET_LOW_BYTE(A):
return (A) & 0xFF
def GET_HIGH_BYTE(A):
return ((A) >> 8) & 0xFF
def BYTE_TO_HW(A, B):
return ((A << 8) | B)
# 舵机ID
ID_ALL = 254 # ID为254时，会向所有舵机进行广播
# 帧头
LOBOT_SERVO_FRAME_HEADER = 0x55
# 指令类型
LOBOT_SERVO_MOVE_TIME_WRITE = 1
LOBOT_SERVO_MOVE_TIME_READ = 2
LOBOT_SERVO_MOVE_TIME_WAIT_WRITE = 7
LOBOT_SERVO_MOVE_TIME_WAIT_READ = 8
LOBOT_SERVO_MOVE_START = 11
LOBOT_SERVO_MOVE_STOP = 12
LOBOT_SERVO_ID_WRITE = 13
LOBOT_SERVO_ID_READ = 14
LOBOT_SERVO_ANGLE_OFFSET_ADJUST = 17
LOBOT_SERVO_ANGLE_OFFSET_WRITE = 18
LOBOT_SERVO_ANGLE_OFFSET_READ = 19
LOBOT_SERVO_ANGLE_LIMIT_WRITE = 20
LOBOT_SERVO_ANGLE_LIMIT_READ = 21
LOBOT_SERVO_VIN_LIMIT_WRITE = 22
LOBOT_SERVO_VIN_LIMIT_READ = 23
LOBOT_SERVO_TEMP_MAX_LIMIT_WRITE = 24
LOBOT_SERVO_TEMP_MAX_LIMIT_READ = 25
LOBOT_SERVO_TEMP_READ = 26
LOBOT_SERVO_VIN_READ = 27
LOBOT_SERVO_POS_READ = 28
LOBOT_SERVO_OR_MOTOR_MODE_WRITE = 29
LOBOT_SERVO_OR_MOTOR_MODE_READ = 30
LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE = 31
LOBOT_SERVO_LOAD_OR_UNLOAD_READ = 32
LOBOT_SERVO_LED_CTRL_WRITE = 33
LOBOT_SERVO_LED_CTRL_READ = 34
LOBOT_SERVO_LED_ERROR_WRITE = 35
LOBOT_SERVO_LED_ERROR_READ = 36
class LobotSerialServo:
def __init__(self, port='/dev/ttyAMA0', baudrate=115200, timeout=0.1):
"""
初始化串口连接
Args:
port: 串口设备路径，默认 '/dev/ttyUSB0'
baudrate: 波特率，默认 115200
timeout: 超时时间，默认 0.1 秒
"""
self.ser = None
self.port = port
self.baudrate = baudrate
self.timeout = timeout
self.connect()
def connect(self):
"""连接串口"""
try:
self.ser = serial.Serial(
port=self.port,
baudrate=self.baudrate,
timeout=self.timeout
)
print(f"成功连接到 {self.port}, 波特率 {self.baudrate}")
except Exception as e:
print(f"连接串口失败: {e}")
raise
def disconnect(self):
"""断开串口连接"""
if self.ser and self.ser.is_open:
self.ser.close()
print("串口已关闭")
def __del__(self):
"""析构函数，确保串口关闭"""
self.disconnect()
def _check_sum(self, buf):
"""
计算校验和
Args:
buf: 字节列表
Returns:
校验和字节
"""
temp = 0
for i in range(2, buf[3] + 2):
temp += buf[i]
temp = ~temp
return temp & 0xFF
def _send_command(self, buf):
"""
发送命令到串口
Args:
buf: 要发送的字节列表
"""
if self.ser and self.ser.is_open:
self.ser.write(bytearray(buf))
def _receive_handle(self, length=32, timeout=0.1):
"""
解析接收到的数据包
Args:
length: 预期接收数据长度
timeout: 接收超时时间（秒）
Returns:
成功返回数据列表，失败返回None
"""
if not self.ser or not self.ser.is_open:
return None
frame_started = False
frame_count = 0
data_count = 0
data_length = 2
recv_buf = [0] * length
start_time = time.time()
while time.time() - start_time < timeout:
if self.ser.in_waiting == 0:
time.sleep(0.001)
continue
rx_byte = ord(self.ser.read(1)) if self.ser.in_waiting > 0 else None
if rx_byte is None:
continue
if not frame_started:
if rx_byte == LOBOT_SERVO_FRAME_HEADER:
frame_count += 1
if frame_count == 2:
frame_count = 0
frame_started = True
data_count = 1
else:
frame_started = False
data_count = 0
frame_count = 0
if frame_started:
if data_count < length:
recv_buf[data_count] = rx_byte
if data_count == 3:
data_length = recv_buf[data_count]
if data_length < 3 or data_length > 7:
data_length = 2
frame_started = False
data_count += 1
if data_count == data_length + 3:
if data_count <= length:
if self._check_sum(recv_buf[:data_count]) == recv_buf[data_count - 1]:
return recv_buf[4:data_count - 1]
return None
return None
def _send_and_receive(self, buf, timeout=0.1):
"""
发送命令并接收响应
Args:
buf: 要发送的命令
timeout: 超时时间
Returns:
响应数据列表或None
"""
# 清空接收缓冲区
if self.ser and self.ser.in_waiting > 0:
self.ser.read(self.ser.in_waiting)
# 发送命令
self._send_command(buf)
# 等待响应
time.sleep(0.01) # 给舵机一点响应时间
# 接收数据
return self._receive_handle(timeout=timeout)
# ========== 舵机控制方法 ==========
def set_id(self, old_id, new_id):
"""
写入舵机ID
Args:
old_id: 当前ID
new_id: 新ID
"""
buf = [0] * 7
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = old_id
buf[3] = 4
buf[4] = LOBOT_SERVO_ID_WRITE
buf[5] = new_id
buf[6] = self._check_sum(buf)
self._send_command(buf)
def move(self, servo_id, position, move_time=1000):
"""
控制舵机转动
Args:
servo_id: 舵机ID
position: 目标位置 (0-1000)
move_time: 转动时间 (ms)
"""
# 限制位置范围
if position < 0:
position = 0
if position > 1000:
position = 1000
buf = [0] * 10
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 7
buf[4] = LOBOT_SERVO_MOVE_TIME_WRITE
buf[5] = GET_LOW_BYTE(position)
buf[6] = GET_HIGH_BYTE(position)
buf[7] = GET_LOW_BYTE(move_time)
buf[8] = GET_HIGH_BYTE(move_time)
buf[9] = self._check_sum(buf)
self._send_command(buf)
def read_id(self):
"""
读取舵机ID（广播读取）
Returns:
舵机ID，失败返回-2048
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = ID_ALL
buf[3] = 3
buf[4] = LOBOT_SERVO_ID_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 2:
return BYTE_TO_HW(0x00, response[1])
return -2048
def read_position(self, servo_id):
"""
读取舵机位置
Args:
servo_id: 舵机ID
Returns:
舵机位置，失败返回-2048
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_POS_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 3:
return BYTE_TO_HW(response[2], response[1])
return -2048
def read_offset(self, servo_id):
"""
读取舵机角度偏差
Args:
servo_id: 舵机ID
Returns:
角度偏差，失败返回-2048
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_ANGLE_OFFSET_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 2:
if response[1] >> 7 == 0:
return response[1]
else:
i = ~response[1] + 1
return -i
return -2048
def read_angle_limit(self, servo_id):
"""
读取舵机角度限位
Args:
servo_id: 舵机ID
Returns:
(下限, 上限) 元组，失败返回(-2048, -2048)
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_ANGLE_LIMIT_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 5:
low_limit = BYTE_TO_HW(response[2], response[1])
high_limit = BYTE_TO_HW(response[4], response[3])
return low_limit, high_limit
return -2048, -2048
def read_voltage(self, servo_id):
"""
读取舵机实时电压
Args:
servo_id: 舵机ID
Returns:
电压值 (mV)，失败返回-2049
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_VIN_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 3:
return BYTE_TO_HW(response[2], response[1])
return -2049
def read_voltage_limit(self, servo_id):
"""
读取电压限制范围
Args:
servo_id: 舵机ID
Returns:
(下限, 上限) 元组 (mV)，失败返回(-2048, -2048)
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_VIN_LIMIT_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 5:
low_limit = BYTE_TO_HW(response[2], response[1])
high_limit = BYTE_TO_HW(response[4], response[3])
return low_limit, high_limit
return -2048, -2048
def read_temperature_limit(self, servo_id):
"""
读取温度报警阈值
Args:
servo_id: 舵机ID
Returns:
温度阈值 (°C)，失败返回-2049
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_TEMP_MAX_LIMIT_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 2:
return BYTE_TO_HW(0x00, response[1])
return -2049
def read_temperature(self, servo_id):
"""
读取舵机实时温度
Args:
servo_id: 舵机ID
Returns:
温度值 (°C)，失败返回-2049
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_TEMP_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 2:
return BYTE_TO_HW(0x00, response[1])
return -2049
def read_load_status(self, servo_id):
"""
读取舵机状态（上电/掉电）
Args:
servo_id: 舵机ID
Returns:
状态 (1=上电, 0=掉电)，失败返回-2049
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_LOAD_OR_UNLOAD_READ
buf[5] = self._check_sum(buf)
response = self._send_and_receive(buf, timeout=0.2)
if response and len(response) >= 2:
return BYTE_TO_HW(0x00, response[1])
return -2049
def stop_move(self, servo_id):
"""
停止舵机转动
Args:
servo_id: 舵机ID
"""
buf = [0] * 6
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 3
buf[4] = LOBOT_SERVO_MOVE_STOP
buf[5] = self._check_sum(buf)
self._send_command(buf)
def set_mode(self, servo_id, mode, speed):
"""
设置舵机模式
Args:
servo_id: 舵机ID
mode: 模式 (0=舵机模式, 1=电机模式)
speed: 速度值
"""
buf = [0] * 10
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 7
buf[4] = LOBOT_SERVO_OR_MOTOR_MODE_WRITE
buf[5] = mode
buf[6] = 0
buf[7] = GET_LOW_BYTE(speed)
buf[8] = GET_HIGH_BYTE(speed)
buf[9] = self._check_sum(buf)
self._send_command(buf)
def load(self, servo_id):
"""
舵机上电
Args:
servo_id: 舵机ID
"""
buf = [0] * 7
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 4
buf[4] = LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE
buf[5] = 1
buf[6] = self._check_sum(buf)
self._send_command(buf)
def unload(self, servo_id):
"""
舵机掉电
Args:
servo_id: 舵机ID
"""
buf = [0] * 7
buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER
buf[2] = servo_id
buf[3] = 4
buf[4] = LOBOT_SERVO_LOAD_OR_UNLOAD_WRITE
buf[5] = 0
buf[6] = self._check_sum(buf)
self._send_command(buf)2.2 获取舵机信息SerialServoGetData.py
import time
from serial_servo import LobotSerialServo
def main():
# 初始化舵机控制器
# 注意：根据你的连接修改串口设备路径
# 常见路径: /dev/ttyUSB0, /dev/ttyACM0, /dev/ttyAMA0 (树莓派GPIO)
servo = LobotSerialServo(port='/dev/ttyAMA0', baudrate=115200, timeout=0.1)
try:
while True:
print("\n" + "="*50)
print("读取舵机信息...")
# 1. 读取舵机ID
servo_id = servo.read_id()
if servo_id == -2048:
print("未检测到舵机，请检查连接！")
time.sleep(2)
continue
print(f"舵机ID: {servo_id}")
# 2. 读取舵机位置
position = servo.read_position(servo_id)
print(f"当前位置: {position}")
# 3. 读取舵机偏差
offset = servo.read_offset(servo_id)
print(f"角度偏差: {offset}")
# 4. 读取角度限位
angle_low, angle_high = servo.read_angle_limit(servo_id)
print(f"角度范围: {angle_low} - {angle_high}")
# 5. 读取电压范围
vin_low, vin_high = servo.read_voltage_limit(servo_id)
print(f"电压范围: {vin_low/1000:.2f}V - {vin_high/1000:.2f}V")
# 6. 读取温度报警阈值
temp_limit = servo.read_temperature_limit(servo_id)
print(f"温度报警阈值: {temp_limit}°C")
# 7. 读取实时温度
temperature = servo.read_temperature(servo_id)
print(f"当前温度: {temperature}°C")
""""""
# 8. 读取实时电压
voltage = servo.read_voltage(servo_id)
if voltage > 0:
print(f"当前电压: {voltage/1000:.2f}V")
else:
print(f"电压读取失败: {voltage}")
# 9. 读取舵机状态
load_status = servo.read_load_status(servo_id)
status_text = "上电" if load_status == 1 else "掉电"
print(f"舵机状态: {status_text}")
print("="*50)
time.sleep(1) # 等待1秒
except KeyboardInterrupt:
print("\n程序被用户中断")
except Exception as e:
print(f"发生错误: {e}")
finally:
servo.disconnect()
if __name__ == "__main__":
main()2.3 控制舵机位置SerialServoRP.py
import time
from serial_servo import LobotSerialServo
print("初始化...")
servo = LobotSerialServo(port='/dev/ttyAMA0', baudrate=115200, timeout=0.5)
# 对应Arduino的loop
try:
while True:
servo.move(1, 1000)
time.sleep(3.0)
position = servo.read_position(1)
print(f"当前位置: {position}")
time.sleep(1.5)
servo.move(1, 500)
time.sleep(3.0)
position = servo.read_position(1)
print(f"当前位置: {position}")
time.sleep(3.0)
except KeyboardInterrupt:
print("\n程序退出")
finally:
servo.disconnect()2.4 修改舵机ID SerialServoIDWrite.py
只能连接一个舵机
import time
from serial_servo import LobotSerialServo
# 对应Arduino的setup
print("初始化...")
servo = LobotSerialServo(port='/dev/ttyAMA0', baudrate=115200, timeout=0.5)
# 对应Arduino的loop
try:
while True:
# 广播设置ID为3
print("\n设置舵机ID: 254 -> 3")
servo.set_id(254, 1) # ID_ALL = 254
# 等待500ms
time.sleep(0.5)
# 读取ID验证
id_result = servo.read_id()
print("")
print(f"new ID: {id_result}")
# 等待1秒
time.sleep(1.0)
except KeyboardInterrupt:
print("\n程序退出")
finally:
servo.disconnect()