184 lines
5.9 KiB
Python
184 lines
5.9 KiB
Python
#!/usr/bin/env python
|
||
# -*- coding: utf-8 -*-
|
||
'''
|
||
# @Time : 2025/12/2 14:05
|
||
# @Author : reenrr
|
||
# @File :servo.py
|
||
# @Description : 控制舵机正反转
|
||
'''
|
||
|
||
from servo_sdk import *
|
||
import logging
|
||
|
||
# -------------参数配置--------------
|
||
BAUDRATE = 115200 # 舵机的波特率
|
||
PORT = 'COM4'
|
||
SERVO_IDS = [1, 2, 3, 4, 5] # 舵机们的 ID 号
|
||
POS_START = 2047
|
||
POS_END = 0
|
||
SPEED = 1500
|
||
ACC = 0
|
||
TIME_INTERVAL1 = ((2047-0) / 1500) + 2 # 翻转回来的时间
|
||
TIME_INTERVAL2 = 10 # 不用翻转运行的时间
|
||
|
||
class ServoController:
|
||
def __init__(self, config=None):
|
||
"""
|
||
初始化舵机控制器
|
||
"""
|
||
self.config = config
|
||
self.time_interval1 = self.config['time_interval1']
|
||
self.time_interval2 = self.config['time_interval2']
|
||
|
||
# 初始化串口和舵机处理器
|
||
self.port_handler = PortHandler(self.config['port'])
|
||
self.servo_handler = HxServoHandler(self.port_handler)
|
||
|
||
# 初始化状态
|
||
self.is_running = False
|
||
|
||
def setup_port(self):
|
||
"""配置串口参数"""
|
||
# 打开串口
|
||
if self.port_handler.openPort():
|
||
logging.info("成功打开串口")
|
||
else:
|
||
raise RuntimeError("打开串口失败")
|
||
|
||
# 设置波特率
|
||
if self.port_handler.setBaudRate(self.config['baudrate']):
|
||
logging.info("设置波特率成功")
|
||
else:
|
||
self.port_handler.closePort()
|
||
raise RuntimeError("设置波特率失败")
|
||
|
||
def enable_all_servos(self):
|
||
"""使能所有舵机"""
|
||
enable_success = True
|
||
for servo_id in self.config['servo_ids']:
|
||
enable_result = self.servo_handler.torqueEnable(servo_id)
|
||
if enable_result[1] != 0:
|
||
logging.info(f"[ID:{servo_id:02d}] 舵机使能失败，错误信息：{enable_result[1]}")
|
||
enable_success = False
|
||
else:
|
||
logging.info(f"[ID:{servo_id:02d}] 舵机使能成功")
|
||
|
||
if not enable_success:
|
||
self.cleanup()
|
||
raise RuntimeError("使能舵机失败,程序退出")
|
||
|
||
logging.info("所有舵机使能成功")
|
||
|
||
def disable_all_servos(self):
|
||
"""失能所有舵机"""
|
||
disable_success = True
|
||
for servo_id in self.config['servo_ids']:
|
||
disable_result = self.servo_handler.torqueDisable(servo_id)
|
||
if disable_result[1] != 0:
|
||
logging.info(f"[ID:{servo_id:02d}] 舵机失能失败，错误信息：{disable_result[1]}")
|
||
disable_success = False
|
||
else:
|
||
logging.info(f"[ID:{servo_id:02d}] 舵机失能成功")
|
||
|
||
if not disable_success:
|
||
self.cleanup()
|
||
raise RuntimeError("失能舵机失败,程序退出")
|
||
|
||
logging.info("所有舵机失能成功")
|
||
|
||
def write_position(self, position, speed, acc):
|
||
"""
|
||
写入目标位置到所有舵机
|
||
:param position: 目标位置
|
||
:param speed: 速度
|
||
:param acc: 加速度
|
||
"""
|
||
for servo_id in self.config['servo_ids']:
|
||
add_param_result = self.servo_handler.syncWritePosEx(
|
||
servo_id, position, speed, acc
|
||
)
|
||
if not add_param_result:
|
||
logging.info(f"[ID:{servo_id:02d}] 添加参数失败")
|
||
continue
|
||
|
||
result = self.servo_handler.GroupSyncWrite.txPacket()
|
||
if result != COMM_SUCCESS:
|
||
logging.info(f"[ID:{servo_id:02d}] 发送指令失败：{result.getTxRxResult(result)}")
|
||
logging.info("复位成功")
|
||
|
||
# 清空参数缓存
|
||
self.servo_handler.GroupSyncWrite.clearParam()
|
||
|
||
def stop(self):
|
||
"""停止舵机运行"""
|
||
self.is_running = False
|
||
|
||
def cleanup(self):
|
||
"""清理资源"""
|
||
self.disable_all_servos()
|
||
if self.port_handler.is_open:
|
||
self.port_handler.closePort()
|
||
logging.info("串口已关闭")
|
||
|
||
|
||
# ---------舵机对外接口----------
|
||
def servo_rotate():
|
||
"""复位位置之后，先转到180度，延时TIME_INTERVAL1一段时间，再转到0度"""
|
||
custom_config = {
|
||
'port': PORT,
|
||
'baudrate': BAUDRATE,
|
||
'servo_ids': SERVO_IDS,
|
||
'pos_start': POS_START,
|
||
'pos_end': POS_END,
|
||
'speed': SPEED,
|
||
'acc': ACC,
|
||
'time_interval1': TIME_INTERVAL1,
|
||
'time_interval2': TIME_INTERVAL2
|
||
}
|
||
|
||
controller = ServoController(custom_config)
|
||
|
||
try:
|
||
# 初始化串口
|
||
controller.setup_port()
|
||
# 使能所有舵机
|
||
controller.enable_all_servos()
|
||
# 复位位置
|
||
controller.write_position(controller.config['pos_end'],
|
||
controller.config['speed'],
|
||
controller.config['acc'])
|
||
|
||
"""运行舵机运动"""
|
||
logging.info("舵机开始循环运动（按Ctrl+C终止）...")
|
||
|
||
# 运动到起始位置（180度）
|
||
controller.write_position(controller.config['pos_start'],
|
||
controller.config['speed'],
|
||
controller.config['acc'])
|
||
logging.info("运动到180度")
|
||
time.sleep(controller.time_interval1)
|
||
|
||
# 运动到结束位置（0度）
|
||
controller.write_position(controller.config['pos_end'],
|
||
controller.config['speed'],
|
||
controller.config['acc'])
|
||
logging.info("运动到0度")
|
||
time.sleep(controller.time_interval2)
|
||
|
||
except KeyboardInterrupt:
|
||
logging.info("\n用户终止程序...")
|
||
controller.stop()
|
||
except RuntimeError as e:
|
||
logging.info(f"\n运行错误：{str(e)}")
|
||
except Exception as e:
|
||
logging.info(f"\n程序运行异常：{str(e)}")
|
||
finally:
|
||
controller.cleanup()
|
||
|
||
|
||
# -----------测试接口------------
|
||
if __name__ == '__main__':
|
||
servo_rotate()
|
||
|
||
|